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I.   INTRODUCTION 

The  methodology  presented  here  is  concerned  with  the  cali- 
bration (more  precisely,  the  maintenance  of  calibration)  of  a 
three-dimensional  tracking  system.   The  individual  sensor  arrays 
are  the  short  base  line  arrays  that  produce  3-D  track  for  NUWES. 
Our  task  is  to  consider  the  coherence  of  track  produced  by  two 
arrays  in  the  regions  of  array  overlap.   These  regions  make  the 
continuous  tracking  of  a  target  achievable.   Thus  many  arrays  may 
contribute  to  the  production  of  track  for  a  single  target  and  the 
integration  of  the  various  track  segments  provided  by  the  indi- 
vidual arrays  is  the  main  problem  in  the  maintenance  of  calibration, 

An  individual  track  segment  produced  by  a  single  array  is 
described  originally  in  the  local  coordinate  system  of  that  array. 
Such  a  segment  must  be  transformed  to  the  Range  coordinate  system 
and  integrated  with  other  transformed  segments  to  form  a  path  for 
the  target.   These  transformations  are  assumed  to  be  linear  and 
require  two  kinds  of  input;  (i)  the  location  of  each  array  in  the 
Range  coordinate  system;  and  (ii)  the  orientation  of  the  local 
coordinate  system  relative  to  the  Range  orientation.   In  underwater 
tracking  the  location  and  orientation  of  a  local  coordinate  sys- 
tem must  be  determined  remotely.   It  is  inferred  from  synchronously 
timed  track  segments. 

Each  array  has  four  sonar  receivers  located  at  four  of  the 
corners  of  a  rigid  cube.   The  target  is  a  torpedo  (or  other 


self-propelled  vehicle) .   It  has  a  "pinger"  attached  to  it  which 
emits  a  sound  wave  at  regularly  timed  intervals  called  point  counts. 
The  position  of  the  target  at  a  given  point  count  is  determined 
from  the  sound  wave's  time  of  arrival  differential  at  the  four 
corners  containing  the  receivers.   These  signals  are  transmitted 
over  cables  to  a  central  computer. 

The  functioning  of  the  Range's  tracking  system  requires  knowledge 
of  the  location  and  orientation  of  each  array.   Moreover,  once  these 
values  are  established,  they  must  be  monitored  regularly  to  guard 
against  slippages  of  various  forms,  i.e.,  calibration  must  be 
maintained. 

There  are  a  number  of  instrumentation,  measurement  and  environ- 
mental problems  associated  with  this  type  of  system,  but  they  are 
of  no  concern  in  the  present  work.   Our  goal  is  restricted  to 
estimating  array  positions  and  orientations.   Mathematically  the 
former  is  a  three-dimensional  vector  and  the  latter  is  a  three  by 
three  coefficient  matrix  that  is  constrained  to  be  orthonormal, 
that  is,  a  rigid  rotation  of  an  array's  cube  in  three  space. 

This  model  for  describing  the  changes  in  processing  the  local 
track  from  an  array  would  be  exact  if  the  speed  of  sound  in  water  we: 
constant.   Such  is  not  quite  the  case.   This  speed  increases  with 
depth  and  is  assumed  to  be  homogeneous  in  each  horizontal  layer  of 
water.   Because  of  the  depth  gradient  there  is  a  non-linear  correc- 
tion term  in  the  vertical  position  of  the  object.   Generally  it 
is  not  very  large.   If  an  array  had  sunk  fifty  feet  deeper  in 
a  muddy  bottom  and  a  typical  speed  with  depth  gradient  were  used 
then  sound  ray  tracing  computations  show  that  the  change  in 


position  of  a  target  some  3000  feet  away  is  within  a  foot  of  that 
computed  using  the  linear  model.   For  NUWES  purposes  it  is  safe 
to  use  the  linear  model  to  detect  slippage.   If  the  non-linear 
term  were  suspected  of  being  important  then  the  estimation 
methodology  could  be  iterated.   That  is,  the  local  coordinate 
system  could  be  corrected  using  the  linear  methodology  followed 
by  a  recomputation  of  the  target's  track.   Then  a  reexamination 
of  the  consequences  of  this  could  be  used  to  decide  whether  the 
local  coordinate  system  should  be  corrected  again. 

It  will  be  seen  that  the  corrections  estimated  for  an  array  are 
not  absolute  but  relative  to  the  other  arrays  in  the  system.   Be- 
cause of  this  it  is  convenient  to  assume  that  the  location  and 
orientation  of  at  least  one  array,  relative  to  the  Range,  were  al- 
ready established.   This  fact  has  implications  in  the  calibration 
maintenance  problem.   That  is,  the  user  places  faith  in  the  current 
official  positioning  of  one  or  more  arrays.   Then  possible  changes 
in  the  positioning  of  others  are  estimated.   If  the  user  chooses 
to  change  his  selection  of  this  "anchor"  array,  the  estimated 
changes  for  the  others  must  be  adjusted  mathematically. 

The  regions  of  overlap  for  two  or  more  arrays  will  be  called 
crossover  regions.   Crossover  data  refers  to  the  tracks  (in  Range 
coordinates)  in  the  crossover  region  which  are  produced  by  two  or  more 
arrays  for  a  common  set  of  point  counts  (i.e.,  time  points).   The 
estimation  of  array  positioning  changes  is  performed  by  attempting  to 
make  crossover  data  agree  as  much  as  possible  in  some  global  sense. 
We  adopt  the  principle  of  least  squares  to  serve  in  this  role. 


Experience  with  the  least  squares  approach  has  revealed  some 
occasional  shortcomings.   There  exist  cases  for  which  the  least 
square  objective  function  is  a  rather  flat  surface  as  a  function 
of  the  parameters  that  describe  the  changes.   (This  has  occurred 
only  when  the  number  of  arrays  in  a  problem  is  two.)   Since  this 
can  result  in  rather  large  estimated  changes  a  second  method, 
called  the  principal  components  method,  has  been  developed.   It 
involves  the  fitting  of  straight  lines  to  the  track  segments  in 
the  crossover  region,  and  this  chosen  array  positions  changes 
that  will  bring  these  lines  together.   It  has  worked  well  for  the 
cases  that  cause  trouble  in  the  least  squares  approach. 

A  brief  comparison  of  the  two  methods  follows:   The  principal 
components  method  does  not  require  crossover  data,  i.e.,  two  or 
more  versions  of  track  for  a  common  set  of  point  counts.   It  does 
require  that  the  two  segments  of  track  being  matched  should  be 
straight.   Also,  if  the  data  are  not  literally  of  the  crossover 
type,  then  one  must  also  estimate  the  distance  between  the  segments 
being  aligned.   The  least  squares  approach  requires  crossover 
data,  but  allows  the  target  to  be  maneuvering  in  the  crossover  regioi 

The  paper  is  organized  as  follows:   The  second  section  con- 
tains the  development  of  the  least  squares  method  for  the  simplified 
case  of  two  arrays  producing  track  in  a  single  crossover  region. 
The  main  features  of  the  method  can  be  presented  in  this  setting. 
In  the  third  section  the  application  of  these  ideas  is  extended 
to  the  general  case  of  multiple  arrays  and  a  multiple  set  of  cross- 
over regions.   After  that  we  develop  the  principal  component 
method--again  treating  the  two  array  problem  first. 


A  Fortran  77  program,  KEYMAIN,  has  been  developed  to  produce 
the  correction  estimates  from  crossover  data.  The  source  codes 
for  this  program  and  all  subroutines  are  contained  in  the  Appendix, 


II.   MATHEMATICAL  PRELIMINARIES 

In  the  formulas  that  follow  z  will  always  be  a  scalar,  A  and 
V  column  vectors,  and  B  a  square  matrix.   The  quantities  3z/3C 
will  have  the  same  algebraic  structure  as  C  and  the  elements 
will  be  the  partial  derivatives  of  z  with  respect  to  the  elements 
of  C.   The  superscript  T  refers  to  matrix  transpose.   The 
following  formulas  are  recorded: 


If  z  =  VTA   then   3z/3A  =   V  (2.1) 

If  z  =  ATA   then   3z/3A   =   2A  (2.2) 

If  z  =  VTBA   then   3z/3B   =   VAT  (2.3) 

If  z  =  VTBTV   then   3z/3B   =   2BWT  (2.4) 


Also  the  trace  of  a  matrix  (sum  of  the  diagonal  elements)  will 
play  a  major  role.  If  B  and  C  are  square  matrices  of  the  same 
order,  liberal  use  will  be  made  of  the  facts  that 

Trace (BC)   =   Trace (CB)  (2.5) 

and  that  the  trace  is  a  linear  operator.   The  formulas  (2.1)  to 
(2.5)  can  be  found  in  Reference  3,  p.  7ff,  and  they  are  not 
difficult  to  develop  directly. 

Consider  a  set  of  point  counts  S  in  a  crossover  region,  and 
let  the  crossover  data  (in  Range  coordinates)  be 


xx(t) 

[   x2(t)  J 

x3(t) 

provided  by  two  different  sensing  arrays.   Let  us  agree  that  the 

y(t)  data  comes  from  the  array  whose  location  and  orientation 

are  established  and  our  goal  is  to  check  the  calibration  of  the 

other  array.   In  particular,  does  there  exist  a  3  vector  A  ^  0,  and  a 

3x3  matrix  B  (7*  I,  the  identity  matrix)  such  that  the  adjusted  track 
values , 

x(t)   =   A  +  B  x(t)  ,  (2.6) 

are  in  better  agreement  with  the  y(t)  than  are  the  unmodified  x(t). 

The  vector  A  is  related  to  a  displacement  of  the  sensing 
array  and  the  matrix  B  is  related  to  a  correction  of  its  orien- 
tation.  If  we  let  5(t)  be  x(t)  in  the  local  coordinate  system,  a  be 
the  location  (in  Range  coordinates)  of  the  array,  and  3  the 
(orthonormal)  orientation  adjustment  to  the  local  coordinates  then 

x(t)   =   a  +  3?(t) 

and 

x(t)   =   A  +  Ba  +  B3?(t)  (2.7) 

The  corrected  location  and  orientation  adjustments  are  A+Ba  and 
B3,  respectively. 


Our  immediate  goal  is  to  estimate  A  and  B  using  the  principle 
of  least  squares.   We  will  minimize  the  average  square  deviation 

between  y(t)  and  A+Bx(t)  for  each  point  count.   Using  the  squared 

2     T 
norm  notation,   ||v||   =  V  V, 


Q   =   Ave   ||y(t)  -A-Bx(t)||2  (2.8) 


and,  since  what  follows  involves  a  modification  of  the  standard 
multivariate  regression  development,  let  us  review  an  outline  of 
the  details. 

First,  the  estimator  for  A  is 

A   =   y  -  B  x  (2.9) 


where  y  =  Ave  y(t)  and  x  =  Ave  x(t) .   The  proof  follows  from  an 

t  t   ~ 

expansion  of  (2.8),  namely 


Q   =   Ave[y(t)  -  Bx(t)]T[y(t)  -Bx(t)j 
t 


-  2  Ave[y(t)  -Bx(t)]TA  +  ATA 


to  which  we  apply  the  rules  (2.1)  and  (2.2), 

|£  =   -2  Ave[y(t)  -  Bx(t)  -  A]  . 
Set  this  equal  to  zero,  solve  for  A  and  the  result  follows 


Next,  let  us  make  a  change  and  work  with  the  deviations 

Y(t)   =   y(t)  -  y    and    X(t)   =   x(t)  -  x  (2.10) 

and  use  (2.9)  in  (2.8).   The  result  is  a  simplified  appearance 
for  Q, 


Q   =   Ave   II Y  (t)  -BX(t)  II  2  (2.11) 


(2.12) 


Also  introduce  the  covariance  matrices 


D     =   Ave{Y(t)YT(t)  }     D     =   Ave{Y(t)XT(t)  } 
yy      t  yX      t 


D     =   Ave(X(t)XT(t)  } 
xx       t 


Now  the  customary  estimator  for  B  can  be  derived  readily.   Thus 


Q   =   Ave{YT(t)Y(t)  -2YT(t)BX(t)  +XT(t)BTBX(t)  }  (2.13) 


and  use  (2.3)  and  (2.4)  to  get 


|§  =   -2  Ave{Y(t)XT(t)  -BX(t)XT(t)} 

9B  t 


-      "2{Dyx   "   BDxx>  <2-14> 


f1 

XX 

which  is  the  usual  multivariate  regression  estimator,  Ref.  1,  p.  181 


using  (2.12) .   Set  this  equal  to  zero  and  solve  for  B  =  D   D 

yx  x^c 


Generally,  the  above  usual  estimator  is  not  the  one  we  should 
use.   Recall  that  the  sensing  arrays  are  rigid  cubes.   If  they 
have  slipped  (i.e.,  moved  physically  such  as  by  the  action  of  a 
ship's  anchor  hooking  a  cable)  then  they  undergo  a  displacement 
and  a  re-orientation.   The  matrix  B  should  be  orthonormal  as  is 
the  matrix  3  in  the  original  calibration.   Thus 


BTB   =   I  (2.15) 


and  the  estimation  of  B  involves  the  minimization  of  Q  subject  to 
the  constraint.   The  usual  estimate  would  be  useful  only  if  there 
were  physical  damage  to  an  array,  resulting  in  its  behavior  as  a 
skewed  rather  than  Cartesion  system. 

Use  of  (2.15)  and  (2.12)  in  (2.13)  allows  the  rewrite 


Q   =   TracelD    -  2D   BT  +  D  v }  (2.16) 

yy     yx      xx 


and  the  minimization  of  Q  is  tantamount  to  the  maximization  of 


W   =   Trace {D    BT}    subject  to  (2.15)  (2.17) 

yx 


since  the  trace  is  a  linear  operator  and  B  is  involved  only  in 
the  second  term  of  (2.16).   The  solution  of  the  optimization 
problem  (2.17)  and  its  generalization  in  Section  4  is  the  bulk 
of  our  effort. 

Earlier  was  stated  the  need  for  placing  faith  in  the  location 
and  orientation  of  at  least  one  array,  because  otherwise  the 


10 


solution  could  not  be  unique.   Let  us  show  now,  using  analysis, 
why  this  is  the  case  in  the  two  array,  single  crossover  region 
problem.   Suppose  we  tried  to  adjust  both  data  sets,  i.e., 

y(t)   =   Ax  +  Bxy(t)     and 

(2.18) 
x(t)   =   A2  +  B2x(t) 


when  both  B-,  and  B2  are  constrained  to  be  orthonormal.   The  least 
squares  objective  function  has  the  appearance 

Q   =   Ave  HA^B^t)  -  A2  -B2x(t)  ||  2  (2.19) 


It  is  clear  from  the  development  of  equation  (2.9)  that  the  dis- 
placement portion  of  the  optimization  can  be  estimated  only 
relatively.   That  is,  only  A  =  A~  -  A,  can  be  found  uniquely. 
From  the  earlier  development  we  can  infer  that  A  =  B,y  -  B„: 
and  re-express  the  objective  as 


.x 


Q(B1,B2)   =   Ave  ||B1Y(t)-B2X(t)  ||  2  (2.20) 


But  this  is  the  squared  lengths  of  a  set  of  vectors  which  have 
been  averaged.   Since  the  squared  length  of  any  vector  is  invari- 
ant under  any  orthonormal  transformation,  say  C,  it  follows  that 


Q(B1,B2)   =   Q(CB1,CB2) 


T 
In  particular  we  can  take  C  =  B,  (i.e.,  the  inverse  of  B,)  and 

T 
then  Q(B, /B „)  =  Q(I,B,).   The  product  of  orthonormal  matrices 
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T 
is  itself  orthonormal.   We  can  set  B  =  B, B~  and  note  that  the 

minimization  of  Q(I,B)  is  that  of  (2.11)r  the  problem  we  are 

treating.   The  geometrical  interpretation  is  that  the  orientation 

of  a  sensing  array  can  be  matched  only  relatively  to  the  other 

array.   It  is  convenient  to  assume  that  the  latter  orientation 

is  known. 
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III.   SOLUTION  ALGORITHM 

The  matrix  B  has  nine  elements,  but  because  of  the  constraint 
(2.15)  there  are  only  three  degrees  of  freedom.   That  is,  there 
are  three  length  constraints  and  three  orthogonality  constraints. 
The  three  remaining  degrees  of  freedom  can  be  expressed  in  terms 
of  the  Euler  angles,  Ref.  2,  p.  250  ff. 

Any  orthnormal  transformation  in  3-space  may  be  viewed  as  the 
application  of  three  successive  rotations,  i.e.,  hold  the  x,-axis 
fixed  and  execute  a  rotation  in  the  x2-x~  plane;  then  hold  the 
x~-axis  fixed  in  its  new  position  and  rotate  in  the  x-,-x   plane; 
and  finally  rotate  in  the  x,-x   plane  with  the  x-.-axis  held  fixed 
in  its  new  position.   Denote  the  angles  of  these  three  rotations 
as  <f>,  /  cj)2  /  <$>->•       In  navigation  work  they  are  called  roll,  pitch 
and  yaw.   These  angles  are  unique  only  when  the  order  of  applica- 
tion of  the  rotations  is  specified. 

To  shorten  the  writing,  let 

c.   =   cos  (<J).)    and    s.   =   sin(<|).)  (3.1) 

for  i  =  1,2,3,  and  the  individual  planar  rotations  can  be  expressed 
as 


0       \  /  c2    0    -s 


;  J  *  ■ ! 


P1  \°         cl      _sl}    p2   =   {  °     X    °   /  (3*2) 

s2    0    c2  ) 


13 


p3 


C3 

"S3 

0 

S3 

C3 

0 

0 

0 

1 

and,  using  the  order  of  application  described,  let 


B   =   P3P2Pi  (3.3) 


We  make  liberal  use  of  the  fact  that  the  product  of  orthonormal 
matrices  is  itself  orthonormal. 

We  note  in  passing  that  the  matrices  B.  above  are  the  trans- 
pose (and  hence  inverses)  of  those  that  are  usually  used  in 
studying  the  motion  of  aeroplanes,  Ref.  [2].   Also  the  order  of 
application  is  the  reverse  of  the  customary  one  thus  making  B  in 
(3.3)  the  inverse  of  the  usual  matrix.   This  choice  is  appropriate 
for  our  work  since  the  angular  correction  will  rotate  the  local 
axes  back  to  where  they  are  expected  to  be. 

This  done  our  optimization  problem  may  be  stated  as 

T 
Maximize  TracelD    B  }  (3.4) 

The  solution  can  be  found  by  treating  the  problem  as  three 
successive  two-dimensional  problems.   To  do  this  let  us  first 
characterize  how  this  optimization  problem  would  be  solved  if 
the  tracking  took  place  in  two  dimensions. 

In  the  plane,  an  orthonormal  matrix  B  has  one  degree  of 
freedom  and  it  is  characterized  by  a  rotation  through  an  angle  <$> . 
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As  before,  let  c  =  cos((j)),  s  =  sin(cf>)  so  that 


!c    -s 
s    c 


and 


W  =   Trace(DBT)   =  ^1i+^22)c   +    (D21  "D12  (s)       (3-5) 


Thus  W  is  itself  a  sine  wave.   It  has  one  max  and  one  min,  and  these  are 
separated  by  it  radians.   Since 


W1   =   -(DX1  +D22)s  +  (D21~D12)c    and 


W"   =   -(DU+D22)C  "  (D21-D12)S 


it  is  seen  easily  that  the  max  occurs  when 


s      =      K(D21   -D-,2)     ,  c      =      K(Dii   +D22)     '       and 


2  2    "1/2 

K      =       {(D21   -D12)       +    (D11+D22)     } 


(3.6) 


This  would  be  the  solution  if  the  tracking  problem  were  a  two- 
dimensional  one. 

Now  we  are  positioned  to  treat  our  three-dimensional  tracking 
problem.   Introduce  matrices 


rn   rp  m        rn  rn   m 

E   =   D   pnp0    F   =   p0D   p,    G   =   pnp0D  (3.7) 

yxKlM2  K3  yxKl  2  3  yx 


15 


and  note  that,  using  (2.5),  (2.17)  and  (3.3), 


T  T 

W  =   Trace(D  B  }     Trace{Ep  -J 
yx  -^ 


=   Trace{Fp2>     =   TraceCGp^"}  (3.8) 


These  latter  three  representatives  of  W  allow  us  to  use  our 
knowledge  of  the  solution  to  the  two-dimensional  problem.   When 
these  three  traces  are  written  out  in  expanded  form  and  compared 
with  (3.5)  and  (3.6),  it  is  seen  that  the  optimal  solution  for 
(J),,  cf>2,  <j>,  must  have  the  form 


s3   =   K3(E21-E12)    and    C3   =   VE^+E^) 


s2      =      K2(F3l"F13)  and        c2      =      K2(Fll+F33)  (3.9) 


sl      =      K1(G32"G23)         and        Cl      =      K1(G22+G33) 


where 


2  2    _1/2 

K3      =       UE21-E12)       +     (E11+E22)2} 


2  2    _1/2 

K2      =       {(F31"F13}       +    (F11+F33)     }  (3.10) 


2  2    "1/2 

Kl      =      {(G32  -G23}       +    (G22+G33)     } 
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The  above  does  not  provide  explicit  solutions  because 
E  =  E(<j>,  ,  <j>2)  i    F  =  F(<h,<J>3)  and  G  =  G  (<J>2 ,  <(>-)  .   It  is  necessary  to 
get  all  three  angles  satisfying  (3.9)  and  (3.10)  at  the  same  time. 
This  means  in  addition  that 

VW   =   0  (3.11) 

as  well.   We  know  that  each  of  the  equations 

T 

8W  dp3 

^—     =      Trace   E   -^     =      0 

9(f>3  3<j>3 

T 

8W  9p2 

|~     =      Trace   F  ^— -     =      0  (3.12) 

T 

3W  dpl 

|~-      =      Trace    G   r~      =      0 

has  two  solutions,  one  max  and  one  min.   It  follows  that  (3.11) 

3 

has  eight  (2  )  solutions,  one  for  each  of  the  permutations  of  the 

component  solutions,  and  this  means  one  max,  one  min,  and  six 
saddle  points.   We  require  an  algorithm  that  converges  to  the  max. 

This  can  be  achieved  with  an  iterative  process.   Take  an 
arbitrary  beginning  set  of  values  for  $,  ,  cf)~,  <j>-..   All  $  .  =  0 
will  do.   Compute  E  for  this  choice  and  then  compute  a  new  <£_ 
from  the  first  member  of  (3.9).   Use  this  value  and  the  original 
<J),  to  compute  F  and  from  this  compute  a  new  $-    from  the  second 
member  of  (3.9).   Next  use  these  new  values  for  $9'  <t>r>    an<3 
compute  G,  followed  by  a  new  <}>-  from  the  third  member  of  (3.9). 
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This  completes  one  cycle.   At  each  step  the  appropriate  member 
of  (3.12)  was  satisfied  and  with  a  max.   The  value  of  W  increased  e; 
time.  Moreover,  a  second  cycle  beginning  with  the  values  <J>^,  cf>2  ,  and  tj). 
from  the  first  cycle  would  produce  a  further  increase  in  W.   We 
know  there  exists  a  unique  maximum  for  W  and  we  have  an  iterative 
method  that  increases  W  at  each  step.   It  follows  that,  by 
repeating  the  process,  we  can  come  arbitrarily  close  to  this 
maximum  and  that  will  be  signaled  when  all  three  members  of  (3.12) 
are  negligibly  small  in  magnitude. 

In  order  to  develop  intuition  that  may  be  useful  in  under- 
standing the  more  complicated  optimization  process  addressed  in 
Section  4,  let  us  take  pause  now  and  make  a  heuristic  interpreta- 
tion of  our  iterative  process.   The  objective  function  W  is  a 
sine  wave  as  a  function  of  each  angle  cf> .  (with  the  other  two 
angles  held  fixed) .   As  such  it  is  concave  in  half  of  this  restrict 
angular  domain.   It  follows  that  in  the  full  three-dimensional 
space  of  (cj),  ,<{>2 ,  <J)^)  the  function  W  is  concave  in  one  eighth  of  its 
domain.   A  more  general  (and  perhaps  more  rapidly  converging) 
gradient  search  algorithm  should  begin  in  this  part  of  the 
domain  so  that  when  convergence  takes  Dlace,  it  is  toward  a 
maximum.   If  the  algorithm  presented  is  used,  one  does  not  have 
to  worry  about  this  point. 

We  note  in  passing  that  the  above  technique  is  not  limited 
to  three  dimensions.   Suppose  that  our  tracking  problem  was  in 

p  dimensions  and  that  B  was  a  p  xp   orthonormal  matrix.   This 

2 
constraint  reduces  the  degrees  of  freedom,  p   in  B  by  p  for  the 
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length  constraints,  and  by  an  additional  p(p-l)/2  for  the 
orthogonality  constraints.   Since 


P2  -P  -p(p-D/2   =   p(p-l)/2 


we  see  that  p(p-l)/2  is  the  number  of  angles  (or  equivalently , 
product  matrices  in  the  analogue  of  (3.2))  of  rotation  in  B. 
I.e.,  it  is  the  number  of  ways  we  can  choose  p-2  axes  (from  p 
axes)  to  be  held  fixed  while  a  rotation  takes  place  in  the  planes 

remaining.   The  solution  structure  will  have  one  max,  one  min, 

p  .  .... 

and  2-2  saddle  points.   Our  iteration  technique  will  converge 

to  the  max. 
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IV.   GENERAL  CASE 

To  treat  the  general  case  we  must  consider  several  arrays,  say 
K  in  number,  and  several  crossover  regions.   Also  we  must  allow  for 
the  target  to  be  tracked  in  a  given  crossover  region  either  not  at 
all,  exactly  once,  or  more  than  once  since  it  may  maneuver  back 
into  a  given  region  during  a  later  point  count  set.   Finally  the 
target,  during  a  given  point  count  set  in  a  given  crossover  region, 
may  be  tracked  by  more  than  one  sensing  array.   We  proceed  to  develo 
a  notational  structure  that  can  handle  this  fully  general  case. 

Let  S-,  ,S~  ,  .  .  .  ,SR  represent  a  collection  of  R  point  count  sets. 

It  is  convenient  to  assume  that  to  each  individual  S„  (for 

s 

s  =  1,...,R)  there  is  associated  a  pair  or  more  of  sensing  arrays. 
Thus,  if  only  one  array  tracks  the  target  in  a  particular  crossover 
region  there  will  be  no  corresponding  point  count  set  in  the  collec- 
tion.  If  exactly  two  arrays  track  the  target  then  the  particular 
S   is  well  defined.   If  three  or  more  arrays  track  the  target  at 
about  the  same  time,  then  things  become  a  little  fuzzy  because  the 
point  count  set  for  one  pair  of  arrays  may  not  be  exactly  congruent 
with  the  point  count  set  of  the  crossover  data  of  one  of  the  two 
arrays  with  a  third  array,  etc.   This  possible  lack  of  congruence 
does  not  affect  the  computation  of  the  cross  covariance,  (2.12)  as 
they  appear  only  in  pairs.   So  there  is  no  harm  in  allowing  the  sets 
S-,,...,SR  to  duplicate  some  segments  of  track.   When  such  duplicatio 
occurs  however,  the  array  pairs  must  be  distinct. 

Next,  for  i  =  1,...,K,  let  C.  be  the  subcollection  of 
Sw . . . ,S_  which  has  tracking  data  from  the  i —  array.   In  this 
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way  we  can  identify  a  3-dimension  vector  of  track  data,  X. (t,s), 
as  being  produced  by  the  i —  array  at  point  count  t  which  belongs 
to  S  .   These  quantities  exist  only  if  S   e  C- . 

Our  goal  is  to  estimate  the  displacement  and  re-orientation 
parameter  pairs  for  each  of  the  K  arrays.   Earlier,  with  K  =  2, 
we  learned  that  there  is  an  identif iability  problem  and  it  was 
convenient  to  assume  that  the  location  and  orientation  of  one 
of  the  arrays  was  fixed.   The  same  is  true  in  the  general  case 
(i.e.,  only  one  array  is  fixed)  provided  that  the  data  satisfy  a 
connectivity  condition.   This  condition  can  be  described  in  the 
parlance  of  graph  theory.   The  K  sensing  arrays  form  the  nodes. 
An  arc  exists  between  two  nodes  if  crossover  data  exists  between 
them.   We  wish  to  consider  only  connected  graphs.   What  this 
means  is:   There  is  no  partition  of  the  nodes  into  two  non- 
empty sets  such  that  an  arc  cannot  be  found  connecting  a  node  of 
one  set  to  a  node  of  the  other  set. 

If  this  condition  of  connectedness  is  not  satisfied  by  our 
problem,  then  the  overall  data  must  be  decomposed  into  smaller 
sets  so  that  it  does  hold  for  each  subset.   Such  decompositions 
are  unique  and  each  member  of  the  decomposition  is  to  be  treated 
separately.   This  done  we  can  fix  our  analysis  on  the  case  that 
involves  a  connected  graph.   Here  it  will  be  seen  that  the  estima- 
tion of  all  displacements  and  re-orientation  parameters  will  be 
unique  once  that  one  set  (i.e.,  a  set  corresponding  to  a  given 
array)  is  specified. 

Our  notation  needs  to  be  expanded.   Let  X. (t;s)  be  the  3- 

.th 
dimensional  track,  in  range  coordinates,  produced  by  the  1 
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array  at  point  count  t  belonging  to  S  .   Our  goal  is  to  estimate 
the  location  vectors  A.  and  re-orientation  matrices  B.  so  that 
(compare  (2.6) ) 


x^tf-s)   =  A±  +  Bixi(t;s)  (4.1) 


are  as  compatible  as  possible  with  the  overall  track  of  the 
target  in  the  range.   Also  let  x. (s)  be  the  vector  of  averages 
of  the  x. (t;s)  taken  over  t  in  S  ,  and 


Xi(t;s)   =   xi(t;s)  -  x± (s)  (4.2) 


be  the  deviations  from  the  mean.   Our  new  objective  function  can 
be  expressed  as  an  extension  of  (2.19) 

Q   =   H  I  N   Ave   ||B.X.  (t;s)  -  B.X.  (t;s)  ||  2       (4.3) 
i<j  s   s        1  x  J    3 

where  the  outer  double  sum  is  over  1  _<  i  <  j  <_   k,  the  inner  sum 

is  over  s  e  C . nC . ,  and  the  average  is  to  be  taken  over  t  e  S  . 
j-   j  s 

The  weights  N   =  number  of  point  counts  in  S   for  the  array 
pair  (i,  j)  . 

We  can  view  (4.3)  as 


Q   =   Q(BlfB2,...,BK)  (4.4) 


and,  because  of  the  connectedness  assumption,   (4.3)  cannot  be 
decomposed  into  the  sum  of  two  terms 
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Qn  B.  ,...,B.  )  and    Q0(B.  ,...,B. 

1   i,       l  '  2   -j        1 

1        p  Jl       Jr 

for  which  the  subscript  sets  i, ,.. . ,i   and  j ,,...,  j   form  a 

partition  of  1,2,...,K.  Now  we  can  reformat  the  argument  used 
at  the  end  of  Section  2.   If  C  is  any  orthonormal  transformation 

then  Q(CB1,CB2,  .  .  .  ,CBK)  =  Q  (B^  .  .  .  ,BR)  .   Using  C  =  B^,  say,  then 


Q(B1,.../BK)   =   Q(I,B^B2,...,B^BK)  (4.5) 


and,  for  convenience,  it  is  assumed  that  the  orientation  of  the 
first  array  is  known,  i.e.,  B,  =  I  in  (4.3). 
Define  covariance  matrices 


Dij(s)   =   Ave{Xi(t;s)X^(t;s) }  (4.6) 


where  the  average  is  taken  over  the  point  counts  t  in  S  . 
These  quantities  exist  only  for  s  e    C-nC-  ?    0.   Then  (4.3)  may 
be  expanded  to  the  more  useful  form 

Q   =   Trace  j    J    j    N  (D..(s)  -2D..(s)BTB  +D..(s)}  (4.7) 

1  <J     s  J       J 

and  again,  because  the  unknown  matrices  {B,  }  appear  only  in  the  second 
term,  we  may  choose  to  maximize 


W   =   J    Trace {D.  .B  .B.  }  (4.8) 

'.- h  XJ     J     1 
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for  1  <_  i  <  j  £  K  and 

D. •   =   7  N   D. . (s)   or   zero  (4.9) 

according  to  whether  the  summation  for  s  e  C.  nC .  has  content, 

or  is  empty. 

A  word  about  the  computation  of  D. • (s)  in  (4.9).   It  seems 

wise  to  use  the  pooled  within  groups  covariance  in  those  cases 

for  which  S   is  the  union  of  rather  diverse  point  counts.   An 
s  c 

example  should  make  the  point.   Suppose  the  array  pair  (i,j) 
tracks  the  target  at  point  counts  {1,...,25}  and  also  {86 ,...,135} 
The  pooled  within  groups  covariance  would  compute  the  two  covari- 
ances  separately  and  then  combine  them  into  one  using  a  weighted 
(by  sample  size)  average.   In  our  example,  the  weights  would  be 
25/75  and  40/75. 

For  each  fixed  value  of  r  (r  =  1,...,K)  the  right  hand  side 
of  (4.8)  contains  an  expression  of  the  form 


W    =    7   Trace {D.  BTB . }  +   7   Trace {D  .BTB  } 
r     i<r        ir  r  i     J^  rj    j  r 


=   Trace{  £   B  D   B^  +   J   D   B^B  }  (4.10) 

i<r  j>r    J  J 


To  shorten  the  writing  let 


E 
r 


I      B.D       and     F    =    J      D  .BT  (4.11) 


i<r  j>r 
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T  T  T  T 

and  use  the  fact  that  Trace{F  B  }  =  Trace{B  F  }  =  Trace{F  B  }. 

r  r  r  r  r  r 

Then 


Wr   =   Trace{  (Er  +F^)B^}  (4.12) 


Also  it  is  interesting  to  note  that  each  term  of  (4.8)  appears 
in  two  and  only  two  distinct  W  .   It  follows  that 


k 

I   Wr   =   2W  (4.13) 


Now  the  vector  of  partial  derivatives  of  W  with  respect  to  the 

three  Euler  angles  in  B  ,  that  is  d>  ,  ,  d>  ~ ,  d>  -. ,  is  the  same  as 
a        r  rl    r2   rr3 

the  gradient  of  W   (with  B,  ,  .  .  .  ,B   , ,B    ,,,..., B,  held  fixed). 
a  r        1     '  r-1   r+1      k 

Moreover,  the  structure  of  (4.12)  is  the  same  as  that  of  (3.4). 

We  know  from  Section  2  that  VW   has  eight  zeros,  exactly  one  of 

which  corresponds  to  a  max,  and  we  have  an  algorithm  that  converges 

to  that  max. 

The  analysis  above  leads  to  the  construction  of  a  gradient 

search  that  can  ferret  out  the  max  of  W.   To  fix  one  array  we  set 

B,  =  I  and  keep  this  throughout.   Choose  starting  matrices  for 

the  set  B~,...,E  .   Compute  E   =  E  (B,  , . .  .  ,B  -,  )  and 
2       k  r     r   1       r-1 

Fr  =  Fr(Br+l"  *  * 'BK)  for  each  r  =  1'2"-"K   <Ei  E  °  E  FK)  •   Use 
the  algorithm  in  Section  2  applied  to  (4.12),  for  each  r,  to 

produce  the  new  Euler  angles,  i.e.,  the  system  {B  }.   Use  these 

to  recompute  the  {E  ,F  }  and  repeat.   Stop  when  the  gradients 

of  all  W   are  sufficiently  small  in  magnitude.   Each  W  will 
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be  at  a  locaJ  maximum  for  its  argument  B  with  all  of  the  other 
orientation  matrices  held  fixed. 

There  are  some  open  questions  about  the  convergence  proper- 
ties of  this  algorithm.   From  an  empirical  point  of  view  it  has 
appeared  to  work  successfully.   Convergence  is  not  monotone  how- 
ever.  In  none  of  our  test  problems  have  the  {B.}  departed  notably 
from  identity  matrices,  and  the  use  of  all  B.  =  I  to  initialize 
the  algorithm  has  provided  satisfactory  results.   For  one  run 
the  initialization  was  chosen  at  random  (in  the  3(K-1)  dimensional 
space  of  Euler  angles)  and  the  time  to  convergence  was  excessive. 
We  know  that  W  has  many  saddle  points  and  that  it  is  a  concave 
function  in  only  a  small  part  of  its  domain. 

Methods  for  initializing  the  algorithm  need  to  be  developed. 
The  following  idea  may  hold  some  promise:   Referring  to  each 
term  of  (4.8) , 

Trace(D. .BTB. }   =   Trace{D. .B . T} , 
ID  D  l  i]  i] 

T 
where  B?.  =  B.B, ,  the  solution  algorithm  of  Section  3  can  be 

used  to  check  whether  B? .  is  near  to  the  identity.   If  so, 

initialize  all  B^  =  I.   If  not,  then  study  of  the  resulting  {B* . } 

may  lead  to  a  good  selection. 

Let  us  return  to  the  objective  function  Q  of  (4.3)  and  use 

(4.1)  in  it  for  purposes  of  estimating  the  location  parameters 

{A  } .   Formally  we  have 

Q   =   J  H  Nq  Ave   ||  A,  +B  .  x,  (t;s)  -A.  -  B  x-  (t;s)  ||  2        (4.14) 
i<i  s   S         1    1~1         J    3-D 
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where  1  <  i  <  j  <  K;  s  e  C •  nC . ;  the  average  is  taken  over  the 
Again  it  is  useful  to  use  terms 


points  counts  t  in  S  ;  and  N  =  number  of  point  counts  in  S  . 


Q    =    7  I  N   Ave  II A-  +B..X.  (t;s)  -A  -B  x.  (t;s)  II  2 

r      . L  L  s     "  1   i~i        r   r-i      " 

i<r  s 

+  I  ^  Ns  Ave  ||Ar+Brx  (t;s)  -A.  -B  x  (t;s)  ||  2        (4.15) 

j  <r  s  "                       J    J~J 


K 
and  recognize  that   £   Q   =  2Q.   In  a  manner  similar  to  the  one 

r=l   r 
that  produced  (2.7)  we  can  develop 


8Qr 

3-^  =   -2   T   J  N  {B-  x.  (s)  -B  x  (s)  -  (A  -A.)} 
3A         .lls      i~i      r~r       r    l 
r        i<r  s 


+  2   J   J  N  {B  x  (s)  -B.x.(s)  -  (A. -A  )}  (4.16) 

>r  ls      s   r~rv      3^3        D  r' 


Setting  (4.16)  equal  to  zero  results  in  a  3. K  linear  system  in 

the  A,  ,.-.  ,A„.   Let  M.   =  [  N   for  s  e  CnC-.   Then  the  left 
1       K        i]    '-   s  i: 

s 

hand  side  of  the  linear  system  is 


-    y    M.A.+iy    m.   +y    m.}a   -    >  m  .a.  4.17 

.  L:         ir  1    .  L         lr  .L-        ri   r    .  t       ri  i 
i<r         i<r      j>r        J  j>r  J    J 


and  the  right  hand  side  is 
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I      B    I   N  x  (s)  +  I      B     Nsx  (g) 
i<r     s  j>r   J  s 


-  Br(  I      I   N&xr(s)  +  I      I   Nsxr(s)}  (4.18) 

i<r  s    ~       j  >r  s 


Notice  that  the  coefficient  matrix  in  (4.17)  is  the  same  for  all 
three  components  of  the  {A  } .   Also  the  columns  of  the  matrix 
add  to  zero  so  that  its  rank  is  K-l.   It  follows  that  the  system 
is  underdetermined.   We  anticipated  this.   Setting  A-,  =  0  will 
allow  a  unique  solution  for  A2,...,AK.   Of  course,  this  choice 
corresponds  to  the  assumption  made  earlier  that  the  location  and 
orientation  of  the  first  array  is  known. 
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V.   PRINCIPAL  COMPONENTS  METHOD 

Experience  with  the  least  square  method  (two  arrays,  Section 
2)  in  the  area  of  underwater  tracking  has  revealed  some  instabili- 
ties.  Several  cases  were  identified  in  which  the  surface  W  con- 
tained some  rather  extensive  relatively  flat  regions  as  a  function 
of  the  three  Euler  angles.   More  specifically,  it  is  possible  to 
move  from  the  maximizing  point  to  a  saddle  point  and  lose  less 
than  one  percent  of  the  value  of  W.   Moreover  the  effect  of  using 
the  maximizing  point  can  be  to  assign  an  absurd  relocation  geometry 
to  the  position  of  the  sensor  (e.g.,  turned  upside  down  and  sus- 
pended without  support  somewhere  in,  or  above,  the  water).   In 
these  cases  use  of  the  saddle  point  involved  only  minor  relocation 
of  the  sensor.   Clearly  this  is  an  unsatisfactory  state  of  affairs. 

These  anomalies  can  happen,  and  there  is  not  yet  any  fully 
reliable  way  to  identify  the  conditions  under  which  they  may 
occur.   They  seem  to  be  related  to  the  inherent  variability  of 
the  tracking  data  coupled  with  the  point  by  point  crossover  data 
matching  approach  utilized  by  the  least  square  methodology.   It 
may  be  that  certain  naturally  occurring  geometric  conditions 
(e.g.,  straight  line  structure  of  track)  may  act  as  a  catalyst 
for  this  phenomenon. 

An  alternative  methodology  has  been  developed  and  it  appears 
to  avoid  producing  the  absurd  solutions  mentioned  above.   It  is 
based  on  the  matching  of  the  first  principal  components  of  the 
covariance  matrices  of  the  tracking  data. 
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Let  us  take  a  moment  to  describe  the  notion  of  first  princi- 
pal component.   Suppose  we  want  to  fit  a  segment  of  straight 
line  to  a  track  of  data  {x(t);t  eS}.   Further  suppose  we  use  the 
minimum  sum  of  squared  projections  (orthogonal)  as  the  mathemati- 
cal criterion  for  choosing  the  line.   The  solution  is  well  known 
(Ref.  1,  p.  272  ff ) .   Let  X   be  the  largest  eigenvalue  of  the 

covariance  matrix  D   and  let  q  be  an  eigenvector  associated  with 

xx 

A.   That  is,  any  solution  of 


Dxxq   =   Aq  (5.1) 


The  following  facts  are  stated  without  proof: 

(i)     The  vector  q  is  a  set  of  direction  numbers  for  the 

desired  straight  line, 
(ii)    The  line  passes  through  the  centroid  x. 

(iii)   The  projected  track  of  x(t)  onto  this  straight  line 

T 
are  the  scalar  values  u(t)  =  q  x(t). 

These  properties  can  be  exploited  for  our  current  problem  in 

the  following  way.   Let  p  be  a  first  principal  component  for  D 

and  q  play  the  same  role  for  D   .   Both  are  assumed  to  be  scaled 

XX 

to  have  length  one: 


i  p'  -  i  =  i  4 


Adopt  as  the  optimizing  criterion:   Choose  B  orthonormal  so  that 


p   =   Bq  (5.2 
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That  is,  the  re-orientation  matrix  is  chosen  so  that  the  two 
fitted  straight  line  segments  have  the  same  direction  numbers. 
It  appears  that  the  mathematical  problem,  p  =  Bq,  is  most 
easily  solved  using  a  constructive  method.   If  p  =  q  then  B  =  I. 
Barring  this,  proceed  as  follows: 

1)  Let  9  be  the  angle  between  p  and  q. 

2)  Form  an  orthonormal  basis  H  =  {h,,L,h.}  such  that  h,  =  q 
h2  is  in  the  plane  of  p  and  q  but  orthogonal  to  h, ; 

h-,  completes  the  basis. 

/ 2 

3)  Letting  c  =  cos  (6)  and  s  =  vl  -c  ,  we  can  use  the  Gram 

Schmidt  process  and  take 


h2 


(q  -  cp) 


4)   Notice  that 


(  0  j   and     Hp   =    Is 


and  that  Hp  can  be  rotated  through  an  (Eulerian)  angle 

T 
e  into  Hq  by  applying  the  matrix  p, (6),  see  (3.2). 

5)   Since  p-^Hp  =  Hq  and  both  p  -.    and  H  are  orthonormal  matrices 


T  T 
it  follows  that  p  =  H  p^Hq.   Thus 


T  T 
B   =   H  P3H  (5.3) 


Notice  that  this  method  does  not  require  crossover  data  in 
the  strict  sense,  i.e.,  matched  pairs  of  track  from  two  arrays 

for  a  common  set  of  point  counts.   The  matrix  D    is  not  needed 

r  yx 
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and  D   and  D   can  be  computed  without  this  requirement. 
However  the  stability  of  the  principal  components  p  and  q 
require  that  the  two  pieces  of  track  be  rather  straight,  i.e., 
no  curvilinear  bias. 

If  the  input  data  are  not  crossover  data,  there  must  be  an 
adjustment  in  the  way  that  we  estimate  the  displacement  parameter 
A.   The  formula  (2.9)  will  not  be  valid  because  the  x  and  y 
values  do  not  refer  to  the  same  time  (i.e.,  point  count).   Our 
immediate  goal  is  to  estimate  the  distance  6  traveled  by  the 
target  from  its  mean  position  y  at  some  time  t   to  its  new  esti- 
mated position  A  + Bx  at  some  later  time  t  .   Let  us  assume  that 
^  x 

the  two  point  count  sets  (one  for  the  {y}  and  one  for  the  {x}) 

are  mutually  exclusive  and  represent  equally  spaced  time  values. 

Since  the  target's  path  is  straight  we  can  average  the  times  in 

the  two  point  count  sets  to  obtain  values  for  t   and  t  .   Then 

y      x 

one  can  form  the  sets  of  projections 


u(t)   =   qTx(t)       and      v(t)   =   pTy(t)         (5.4) 


and  use  their  successive  differences  to  get  an  estimate  of  the 

target's  speed. 

The  distance  6  will  be  the  speed  multiplied  by  the  time 

increment  t  -  t  . 
x    y 

This  done,  it  is  claimed  that 

A   =   y  -  Bx  +  6p  (5.5) 
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Proof:   The  lineal  distance  6  can  be  represented  as 


T     —  — 
p  (A  +  Bx  -y)   =   6 


T      T  —   — 
from  which  it  follows  that  p  A  =  p  (y  -Bx)  +6.   We  can  view  p 

as  the  first  column  of  a  seld  adjoint  matrix  P,  and  letting 

T 
6  =  (6,0,0)   we  can  express  our  equation  in  the  form 


PTA   =   PT(y  -Bx)  +   6  (5.6) 


From  (5.6)  it  follows  that  A  =  (y  -Bx)  +  P6  and  this  is  the 

same  as  (5.5). 

It  should  be  noted  that  the  principal  components  method  could 

have  been  used  in  place  of  the  least  squares  approach.   Having 

crossover  data  in  hand  does  not  preclude  its  use.   Thus  t   =  t 

c  y     x 

and  6  =  0  in  (5.5) .   We  can  modify  the  objective  function  Q  of 
(2.11)  and  get  a  perfect  fit,  i.e.,  because  of  (5.2), 

2       T       T       T 

0   =    ||p  -Bqj|     =   p  p  -  2p  Bq  +  q  q 

=   2-2  Trace {pqTBT}  (5.7) 

and  the  Trace  factor  of  (5.7)  is  unity. 

With  the  above  in  mind  let  us  turn  to  the  question  of  extend- 
ing the  principal  components  method  to  several  arrays  and 
several  crossover  regions.   It  is  simpler  if  we  have  crossover 
data  so  let's  describe  that  situation  first. 


33 


The  notation  is  the  same  as  in  Section  4.   Let  q. (s)  be 
the  first  principal  component  of  D.^s)  (see  (4.6)).   The 
orientation  changes  {B.}  will  be  selected  first  by  minimizing 


Q   =  I    I    I    ||qi(s)Bi  -q^(s)B.  ||  2 
i<j  s  J    J 

=  I    I    l{2-2    Trace[qi(s)qT(s)BTBi]  }  (5.8) 

i<j  s  1  J 

and  the  summations  are  for  1  <_  i  <  j  <_    K  and  s  e  C^nC..   As 
before,  this  is  tantamount  to  maximizing 


W   =  I    I    Trace{Di.B^Bi}    with    D±.   =  I    qi(s)qT(s)     (5.9) 
i<j         -1  ■>  -1     s       -1 

Since  this  has  the  same  structure  as  (4.8),  the  same  solution 
algorithm  can  be  used.   Also  the  system  (4.17)  and  (4.18)  can  be 
used  to  obtain  the  {A,}. 

The  procedure  changes  a  bit  when  the  data  are  not  of  the 
crossover  form.   That  is,  when  we  are  splicing  together  segments  of 
track  and  there  are  no  overlaps.   One  might  liken  this  problem 
as  being  similar  to  putting  in  water  pipes  in  a  large  building. 
Several  subcontractors  have  put  in  their  pipes  and  our  job  is  to 
connect  it  all  up.   The  pieces  must  be  moved  so  that  the  ends 
butt  up  against  one  another. 

With  this  view,  we  need  a  change  in  terminology.   Let 
S1'°"*'SR  rePresent  tne  crossover  points,  or  connection  points. 
To  each  Ss  there  corresponds  two  arrays,  i.e.,  an  unique  (i,j) 
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pair.   Let  T.(s)  and  T  •  (s)  be  the  point  counts  that  produce  the 
data.   It  is  assumed  that  they  do  not  overlap.   Each  is  suffi- 
ciently extensive  so  as  to  provide  stable  estimates  for  the 
covariance  matrices  D.-(s)  and  D..(s),  yet  not  so  extensive  as 
to  compromise  the  assumption  that  the  target  is  moving  in  a 
straight  line  while  in  the  point  counts  neighboring  the  connec- 
tion.  This  done,  we  can  input  equation  (5.9)  and  use  the  algorithm 
of  Section  4  to  estimate  the  {B-}. 

It  remains  to  estimate  the  {A.}.   To  do  this  the  technique 
preceeding  equation  (5.5)  needs  to  be  expanded.   With  more  than 
two  arrays  involved,  the  objective  function  Q  of  (5.8)  will  not 
be  zero.   This  means  that  our  reoriented  segments  of  track  will 
not  share  an  exact  common  line  at  the  connection  points.   With 
this  possibility  in  mind  we  introduce  some  further  quantities. 

To  each  of  the  sets  T-(s)  and  T • (s)  adjoin   (if  necessary)  a 
common  time  value  (or  pseudo  point  count)  t. •  (s)  which  will  serve 
as  the  connecting  time.   Let  t. (s)  and  t.(s)  be  the  average 
values  of  T-(s)  and  T.(s)  respectively.   For  convenience  it  is 
assumed  that  the  former  preceeds  the  latter;  t-(s)  <  t . (s)  .   The 
corresponding  pre-adjusted  positives  are  x-(s)  and  x.(s).   Esti- 
mate the  target's  speed  in  the  same  array  as  before  and  use  it 
to  compute  the  distances: 

6.(s)   =   distance  traveled  from  t-(s)  to  t.-(s) 


6 • (s)   =   distance  traveled  from  t..(s)  to  t . (s) 
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These  will  be  used  in  our  immediate  goal,  namely,  to  take  the 
linearly  adjusted  pieces  of  track 

Xj_(t;s)   =   Ai  +Bixi(t;s) 

(5.10) 
Xj  (t;s)   =   A-  +B.  x.  (t;s) 

and  model  the  differences  A.  -A.  in  terms  of  the  6. (s) ,  6. (s) 
and  other  known  quantities. 

In  continuing,  let  us  drop  the  argument  s  from  the  notation. 
Again  we  can  use  the  eigenvectors,  q • ,  to  project  the  track  to 
an  axis  so  we  can  measure  the  distances.  Specifically,  write 


qTx.  (t.  .)  -  qT[A-  +B.  x.  ]   =   6  . 


q^[Aj+BjXj]     -    q^(t±j)        =       fij 


(5.11) 


Although  we  don't  know  how  to  compute  x- (t. •)  and  x-(t. ■)  they 
both  represent  the  position  at  the  connecting  point, 
^^ii^  =  xi^tii^'   Next  rewrite  (5.11)  as 


qV   =   q^ft..)  -  q*Bjx.  +  Sj 


T  T~  T   — 

q.A.   =   q. x. (t. • )  -  q.B . x.  -  6 . 
11      ^1  l   ij     ^lii     i 


(5.12) 


Then  apply  the  same  technique  as  used  in  the  proof  of  (5.5). 
The  result  is 
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A.   =   x- (t. .)  -  B-x-  +  q-6 . 

(5.13) 

Ai  ■  *i(tij>  -  Bi*i  -  Vi 

The  unknown  quantities  x- (t. .)  will  cancel  when  we  take 
differences.   Let  us  restore  the  s  to  the  notation  and  record 
the  result. 

A.  -Ai   =   B^Cs)  -  BjX.  (s)  +  5j(s)q.  (s)  +  5i(s)qi(s) 

(5.14) 

The  system  (5.14)  is  overdetermined  and  has  no  solution.   Let  us 
develop  the  least  squares  compromise.   To  shorten  the  writing  let 
g.-(s)  represent  the  right  hand  side  of  (5.14).   Many  details 
will  be  omitted  because  we  follow  the  pattern  at  the  end  of 
Section  4. 
Let 


-III    II A,  -A.-g i,(s)\\2  (5.15) 

i<j  s    J 


and 


Qr   =  I    1    II|Ar-A.-g  ..(s)||2  +  I    I    I||A  -Ar-g  .(s)||2 
i<r  s  J  j>r  s 


Then 
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!§-  =    y  y  [{a  -a.  -g.  (s)  > 

3A      h    l    L      r        1   3ir 
r     i<r  s 


"  I  I  I  {A,  -Ar-9ri(s)}  (5.16) 

j  >r  s   J         J 


which  is  set  equal  to  zero  for  each  r  =  1 , . . .  ,  K.   Let  n .  .  be 

the  number  of  (i,j)  connections  that  exist,  i.e.,  n- ■  =  \    If 

1-1    s 
and  proceed.   For  r  =  1,...,k  we  have  the  system 


-  I  n->-A.+A{y  n-  +  1  n  -  •  }  -  J  n  •  A . 
.L  "ir  i  r  .L  lr  .L  ri  . '-  ri  i 
i<r  i<r       j>r        J  j>r        J  J 


I  I  gir(s)  -  I     I   gri(s)  (5.17) 

i<r  s  j>r  s    J 


and  the  g.  (s)  and  g  . (s)  can  be  found  from  the  right  side  of 
(5.14).   As  before,  the  columns  of  the  coefficient  matrix  add  to 
zero  and  the  rank  of  the  system  is  K-l.   We  set  A,  =  0  (i.e., 
the  location  of  the  first  array  is  assumed  known)  and  solve. 
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c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


c 
c 


c 
c 
c 
c 
c 


PROGRAM  KEYMAIN 

*************************************************************** 

***   This  serves  as  a  main  program  to  call  the  subroutines 
used  to  estimate  the  sensor  array  displacements  and 
the  array  re-orientation  angles  for  the   Keyport  range 
calibration  project.   It  reads  in  data  from  a  disk 
file  specified  by  the  user.  It  prompts  the  user  for 
this  data  and  for  sensor  array  I.D.  information. 

--  26  August  1985 
*************************************************************** 


*** 
*** 
*** 
*** 
*** 
*** 


**} 
**  •; 
*** 
*  *  * 
** 
***! 
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...   Declare  all  integer  variables: 

INTEGER*4  IND2(2,30),  IND1(30,30),  Rl,  NUMREC,  I,  J,  IDL, 

2  K,  IA(30),  TESTC,  NSK30),  IND(30,30),  R,  NS(30), 

3  FF(30,30),  IK,  KM,  IDR,  IND2R(2,30),  DATSET(30), 

4  CHOICE,  OUT,  IN 

...   Declare  all  real  variables  in  double  precision: 
REAL*8  AA(200,6),  CROSSA (3 0 , 3 , 3 ) ,  MEAN(30,6),  DEV(30,3,3), 

2  XB(30,6),  EP,  E(30,3,3),  BB(3,3),  XBB(30,6),  A(30,3), 

3  DEL(30,3),  D(30),  P(30),  C(3,3),  RM(6),  PP,  EA(3C,3) 

...   Declare  the  character  variables: 
CHARACTER  DSNAME*13,  ANSWER*3 

LOGICAL  EXST 

PARAMETER(0UT=6, IM=5) 

WRITE(0UT,*)  '  This  is  a  user  friendly  program.  Hello  !!.  ' 
WRITE (OUT,*)  »    ' 
Rl  =  0 

...  The  IKD2  and  IND2R  matrices  serve  to  store  array  pair- 
identification  information  and  relate  it  to  the  input 
(six  column)  crossover-  data  sets. 

...  Initialize  IND2  and  IND2R  matrices  to  zero: 

DO  30  I  =  1,2 

DO  30  J  =  1,30 

IND2(I,J)  =  0 

IND2R(I,J)  =  0 

CONTINUE 


WRITE (OUT,*)  '  Please  enter  the  name  of  the  da 


a  s&'C  u  n  l  i  s k  : 


...   Rl  counts  the  number  of  cata  sets  input  by  the  user. 
Rl  =  Rl  +  1 
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10 


15 


READCIN, ' (A)  '  )  DSNAME 
...  Check  to  make  sure  data  file  exists: 
INQUIRE (FILE=DSNAME,EXIST=EXST) 
IF(.N0T.  EXST)  THEN 

WRITECOUT,*) '  The  file  does  not  exist.' 

WRITE(OUT,*) '  Do  you  want  to  (1)  try  again  or  (2)  abort?' 
READdN,*)  CHOICE 
IF(CH0ICE  .EQ.  1)  THEN 
Rl  =  Rl  -  1 
GOTO  5 
ELSE  IF(CHOICE  .EQ.  2)  THEN 

WRITECOUT,*)'  Program  terminating  at  your  request.' 
STOP 
ELSE  IF(CHOICE  ,NE.  1  .AND.  CHOICE  .NE.  2)  THEN 
WRITE(OUT,*) '  Please  enter  integer  1  or  2  .' 
GOTO  9 
END  IF 
END  IF 


OPEN(l,FILE=DSNAME, STATUS='OLD' ) 

..  Read  data  from  file  and  count  number  of  records  CNUMR 

NUMREC  =  0 
NUMREC  =  NUMREC  +  1 

READ(1,*,END=20,ERR=15) (AA( NUMREC, J ) , J =1,6) 
GOTO  10 

...   Notify  operator  of  file  read  error: 


EC)  : 


WRITE (OUT,*) 
WRITE (OUT,*) 
WRITE(0UT,*) 
WRITE (OUT,*) 
WRITE(0UT,*) 


An  error  was  detected  in  reading  the  fil 
but  execution  will  continue  anyway  . 


e,  ' 


20 


NUMREC  =  NUMREC  -  1 

WRITE(0UT,*)  '  There  are  », NUMREC'  records  In  date  set 

CL0SE(UNIT=1) 

...  The  number  of  records  per  input  date  set  are  uccu.-ul 

in  the  vector  N  S 1 . 
NSKR1)  =  NUMREC 

...  Define  left  &  right  sensor'  no.'s  for  matrix  IND2: 

"left"  will  be  indexed  with  a  one  and  "right"  wi 
be  indexed  with  a  two. 

WRITE (CUT,*)  '  Name  your  left  sensor  array  number:  ' 

READ (IN,*)  IDL 

WRITE (OUT* *)  '  Name  your  right  sensor  array  number:  ' 

READ (IN,*)  IDR 


tfc  d 


...  The  left  and  right  array  ID's  are  entered  it1  the 
first  and  second  (respectively)  reus  of  I M D  2 . 
IND2(1,R1)  =  IDL 

I I-i D 2  (  2  ,  R 1 )  -  IDR 
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c 
c 
c 
c 
c 
c 
c 


50 


60 


C 
C 

c 
c 
c 
c 
c 
c 
c 

p 

\^ 

c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


65 


be 


Begin  subroutine  calls: 

Subroutine  CROSSP  takes  each  input  data  set  AA,  which 
records*  and  returns  RM  -the  six  component 
C-the  three  by  three  matrix  of  sums  of  crosj 
from  the  means.   These  are  computed  sequent 
in  the  arrays  MEAN  and  CROSSA  as  they  are  a 


has  NUMREC 
vector  and 
deviations 
and  stored 


CALL  CROSSP(NUMREC,AA,RM,C) 

DO  50  I  =  1,3 

DO  50  J  =  1,3 

CR0SSA(R1, I, J)  =  C(I, J) 

CONTINUE 

DO  60  KM  =  1,6 

MEAN(R1,KM)  =  RM(KM) 

CONTINUE 


WRITEC0UT,*)  '  Would  you 
READCIN, ' (A)  '  )  ANSWER 
IF  (ANSWER  .EQ.  'YES'  .OR. 
IF  (ANSWER  .EQ.  'yes'  .OR. 


like  to  call  another  data  set  ?  ' 


ANSWER 
ANSWER 


•  EQ. 
.EQ. 


i  Y»  ) 

fyf  ) 


GOTO 
GOTO 


The  subroutine  C0NECT  (Gygax)  takes  as  input  the  number 
of  data  sets  Rl,  and  the  left/right  array  ID  i.iatri 
IND2  and  determines  whether  the  problem  input  by  til 
user  is  connected.  That  is,  all  sensor  drrdys  in  ta 
problem  communicate  with  one  another  via  a  string  c 
crossover  data  sets.   If  this  test  fails,  then  the 
output  variable  TESTC  is  zero  ana  the  ustr  is  prom  pi 
to  use  one  of  the  three  options  listed  in  the  WRITE 
statements  below. 


If  t 
the 


CALL  CONE 
IF  (TESTC 
WRITE ( 
WRITE ( 
WRITE ( 
WRITE ( 
WRITE( 
WRITE ( 
READU 
IF  (  .  N 
GOTO  ( 
WRITE ( 
STOP 


he  p  r 
sub  ro 

K, 
IND 

IA, 

IND 

DAT 

CT(0U 
.EQ. 
OUT,* 
OUT,* 
OUT,* 
OUT,  * 
OUT,* 
OUT,* 
N,  *) 
0T.  ( 
66,  6 
OUT,  * 


obi  e 
ut  i  n 
the 
1,  a 

the 

ma  i 
2R, 

con 
SET, 

to 
T,R1 

0) 
)  'A 
) 
) 
) 
) 
) 
I 

(I  . 
7,  6 
)  »P 


G    b<= 


'D 
i 

? 

? 

»P 


m  is  connected,  then  TESTC  is  not  zero  an 

e  returns  a  value  for: 

number  of  sensor  arrays  in  the  problem. 

useful  conversion  of  I N D 2 . 

list  of  sensor  ID's  in  the  order 
ntained  by  the  program. 

the  submatrix  of  IND 2  that  represents  cat 
nectec  to  the  first  data  set. 

the  number  of  crossover  data  sets  connected 
the  first  input  acta  set. 
, IND 2, K, IND 1, I  A, TESTC, IND 2 R, DATS ET) 
THEM 

11  of  your  arrays  are  not  connected.  ' 
o  you  want  to  :f 

(1)  Quit  now' 

(2)  Input  more  data' 

(3)  Continue,  using  the  first  connected  se 
lease  enter  the  number  of  your  choice  ' 

EQ.  D.OR.d  .EQ.  2). OR. (I  .EQ.  5)))  GOTO  65 

8),  I 

rogram  termi nati ng .  Adios,  Ami  go  !' 
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67  GOTO  5 
C 

C  ...  The  subroutine  REDUCE  (Gygax)  does  nothing  unless  option 

C  above  is  evoked.   In  that  case  it  modifies  the 

C  variable  CROSSA,  MEAN,  Rl,  K,  IND1,  and  IA  so  that 

C  they  contain  only  the  information  required  by  the 

C  connected  problem  called  for  by  this  option. 

C 

68  CALL  REDUCE(CR0SSA,MEAN,R1,K, IND1,IA, IND2R,DATSET) 
END  IF 

C 

C  ...  Feed  the  arrays  output  by  CONECT  or  REDUCE  into  POOL: 

C  The  subroutine  POOL  checks  on  the  number  of  data  sets 

C  associated  with  each  pair  of  sensor  arrays.   If  this 

C  value  is  one  or  zero  it  does  nothing.   Otherwise  it 

C  (in  effect)  pools  all  the  data  associated  with  each 

C  unique  sensor  array  pair  into  one  data  sex  (using  trie 

C  within  groups  sum  of  squares  technique  for  crossproc'uct 

C  and  the  weighted  average  technique  for  means.   Thus/ 

C  CROSSA  is  converted  to  DEV;  MEAN  to  XD;  Rl  to  R;  IND1  t. 

C  IND;  and  NS1  to  N S .   The  first  six  called  variables  are 

C  input  and  the  last  five  are  output. 

C 

CALL  P00L(K,R1, IND1 , CROSS  A, ME AN , MSI , IND , DEV , XB, R, MS ) 
C 

C  ...  Array  IND  output  by  POOL  goes  into  INVIND: 

C  The  subroutine  INVIND  takes  the  variables  K,  P. ,  c  n  c  IND 

C  and  outputs  the  K  by  K  matrix  FF.   This  is  an  upper 

C  triangular  incidence  matrix  which  contains  a  one  in 

C  the  position  i , j  (  i  <  j  )  only  if  there  exists  a  set  of 

C  crossover  data  involving  both  the  1-th  and  j-ti 

C  sensors  (as  indexed  in  IA).   FF  is  essentially  an 

C  inverse  of  IND. 

CALL  IMVIND(K,R, IND,FF) 
C 

C  ...  Array  F F  is  output  from  INVIND  and  input  t c  MU L T AR . 

C  ...  Set  epsilon,  the  tolerable  error  level  used  in  the 

C  stopping  rule  of  the  subroutine  M  U  L  T  A  R : 

EP  =  l.D-6 
C 

C  ...  The  subroutine  MULTAR  computes,  cy  iteration,  the  K 

C  o  rthono  n,.al  re-orientation  matrices  for  each  sensor 

C  listed  in  IA.   Since  these  are  all  relative  to  the 

C  first  sensor,  the  first  matrix  in  the  output  B  is 

C  the  3  x  3  i cent ity  matrix. 
C 


r. 


CALL  MULTAR(EP,FF,DEV,    K,R,B) 


C        ...  Array  B  is  output  from  MULTAR  and  input  to  INTCEP: 
CALL  IMTCEP(FF,NS,B,XB,K,R, A) 
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c 
c 
c 
c 
c 
c 
c 
c 
c 
c 

c 
c 
c 
c 


70 


c 
c 
c 
c 
c 
c 
c 

c 

c 
c 


...  Array  A  is  output  from  INTCEP  and  input  to  DISPLC: 

...  The  subroutine  DISPLC  takes  variables  K »  B ,    A  *  anc  IA  as 

input*  reads  the  sensor  array  location  file  and  com put 
the  estimated  displacement  of  each  of  the  K  sensors  in 
the  problem.   The  output  DEL  is  the  set  of  displacemen' 
vectors  and  D  is  the  corresponding  set  of  K  d i s p 1 aceme 
distances.   Since  these  are  all  relative  to  the  first 
sensor*  the  first  row  of  DEL  and  first  element  of  D  ail 
zero. 

CALL  DISPLC(K*B* A,IA*DEL,D) 

...  Prepare  the  sequential  computation  of  the  maximum  angles 
of  sensor  rotation. 


DO  75  IK=1*K 
DO  70  1=1*3 
DO  70  J=l*3 
BB(I,J)  =  B(IK* I, J) 
...  Array  BB  is  input 


to  MAXANG 


...  The  subroutine  MAXANG  takes  a  3  by  3  orthonorma 1  matrix  EB 
and  computes  PP*  the  largest  possible  rotation  angle 
experienced  by  any  vector  transfer  rued  by  BB. 

CALL  MAXANG(BB,PP) 
P(IK)  =  PP 
CONTINUE 

...  The  subroutine  ANGLES  converts  each  of  the  K  orthorrorrnal 

matrices  in  B  to  their  representation  as  the  three 
Euler  angles:   Roll*  Pitch*  and  Yaw.   Since  these  are 
relative  to  the  first  sensor,  the  first  row  of  tin's 
matrix  is  zero. 

CALL  ANGLES(K,B*EA) 


...  Prepare  to  write 
0PEN(3*FILE= 'KEYFIL1 
0PEN(4,FILE=»KEYFIL2 
0PEN(7,FILE='KEYFIL3 


the  output  to  files: 
DAT' ,STATUS= 'OLD' ) 
DAT' *STATUS=  'OLD'  ) 
DAT'*STATUS= 'OLD' ) 


0PEN(8,FILE= 'KEYFIL4.DAT' , STATU S= 'OLD' ) 


77 


WRITE(0UT**)  '  Displacement  and  rotation  (magnitude):' 

DO  77  IK  =  1,K 

...  Write  the  vectors  IA*  D  and  P  to  FILE1 

WRITE(3,85)  IA(IK),  D(IK),  P(IK) 

WRITE(0UT,85)  IA(IK),  D(IK),  P(IK) 

...  IV  rite  the  vector  IA  and  matrices  A  and  DEL  to  FILE2. 
WRITE(4,80)  IA(IK),  ( DEL ( IK, L ) , L=l , 3 ) ,  ( EA ( IK, LK) , LK=1 , 3 ) 

CONTINUE 


46 


c 
c 


WRITECOUT,*)  '  Displacement  vectors  and  Euler  angles  :' 

DO  78  IK  =  1,K 

WRITE(0UT,80)  IA(IK),  ( DEL ( IK, L ) , L=l , 3 ) ,  ( E A( IK, LK ) , LK=  1 ,  3  ) 
78      CONTINUE 

80      FORMAT ( IX, 15, IX, 3 F 12. 4, IX, 3 ( IX, F10. 7) ) 
85      FORMAT (2X, I5,2X,F10.2,2X,F10.6) 

...  Write  the  three  dimensional  array  B  to  FILE3. 

WRITE(0UT,*) '   ' 

WRITE(0UT,*) »  Array  B  output  by  KEYMAIN:  ' 

DO  90  IK  =  1,K 

WRITE(0UT,*) '  • 

WRITECOUT,*) »  ' 

DO  90  I  =  1,3 

WRITE(7,100)(B(IK,I,J),J=1,3) 

WRITE (OUT, 100) (B(IK,I,J),J=1,3) 
90      CONTINUE 
100     F0RMAT(2X,3F15.10) 


105 
115 


DO  105  IK  =  1,K 
WRITE(8,115) (A(IK,J),J=1,3) 
CONTINUE 
FORMAT(2X,3F15.5) 


WRITE(OUT,110) 
110     FORMAT(//,*  Program  terminating.   Operation  complete.   Eye.  M 

CLOSE(UNIT=3) 
CL0SE(UNIT=4) 
CL0SE(UNIT=7) 
CL0SE(UNIT=8) 


STOP 
END 


SUBROUTINE  CROSSP  ( N, AA, MEAN, CROSSA) 


*****  a***************************************************** ******* 


***  THIS  PROGRAM  CALCULATES  THE  CROSSPRODUCT  DEVIATIONS  FOR 

***  THE  RAW  DATA  ARRAY  AA  (A  MATRIX  FOR  EACH  CROSSOVER  SET) 

***  THE  SET  OF  MEANS  IS  DEVELOPED  AND  LABELED  "MEAN" 

***  CROSSA  IS  THE  SUM  OF  CROSSPRODUCT  DEVIATIONS  FROM  THE  MEAI 


*  * 

*  * 

*  * 

*  * 


****************************************************************** 


INTEGERM    N 

REAL*6    AA(200,6),     MEANC6),     CR0SSA(3,3),     SUM 

REALMS  SUMXX,  SUMXY,  SUMXZ,  SUMYX,  SUMYY,  SUMYZ,  SUI 

REAL*6   SUMZZ,  XX,  XY,  XZ,  YX,  YY,  YZ,  ZX,  ZY,  ZZ 


;zx,  SUMZY 


N  is  the  number  of  rows  in  the  data  matrix  A A 
Compute  the  vector  of  means. 
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DO  20  J  =  1,6 
SUM  =  O.DO 


DO  10  I  =  1,N 

SUM  =  SUM  +  AA(I,J) 

10 

CONTINUE 

MEAN(J)  =  SUM  /  DBLE(N) 

20 

CONTINUE 

C 

C 

C 

...  Begin  the  crossproduct  operations: 

C 

Initialize  the  nine  sums  to  zero. 

SUMXX=0.D0 

SUMXY=0.D0 

SUMXZ=0.D0 

SUMYX=0.D0 

SUMYY=0.D0 

SUMYZ=0.D0 

SUMZX=0.D0 

SUMZY=0.D0 

SUMZZ=0.D0 

DO  100  IA  =  1,N 
C 

C      ...  Create  the  crossproduct  deviations  for  row  IA  of  matrix  AA 
C 

XX=(AA(IA,1)-MEAN(1) ) * ( A A ( I A, 4 ) -MEAN ( 4 ) ) 

XY=(AA(IA,1)-MEAN(1) ) * ( A A ( I  A, 5 ) -ME AN ( 5 ) ) 

XZ=(AA(IA,1)-MEAN(1) ) * ( AA ( I  A, 6 ) -ME  AN ( 6 )  ) 

YX=(AA( IA, 2) -MEAN (2) ) * ( AA ( I  A, 4 ) -ME  AN ( 4 ) ) 

YY=(AA(IA,2)-MEAN(2) ) * ( AA ( I  A, 5 ) -MEAN ( 5 ) ) 

YZ=(AA(IA,2)-MEAN(2) ) * ( AA ( I  A, 6 ) -ME AN ( 6 ) ) 

ZX=(AA(IA,3)-MEAN(3) ) * ( AA ( I A, 4 ) -ME AN ( 4 ) ) 

ZY=(AA(IA,3)-MEAN(3) ) * ( AA( IA, 5 ) -MEAN ( 5 ) ) 

ZZ=(AA(IA,3)-MEAN(3) ) * ( AA ( I  A, 6 ) -MEAN ( 6 ) ) 

the  s  u  in  s  : 


C 

C 

...  Accumulate 

SUMXX=XX+SUMXX 

SUMXY=XY+SUMXY 

SUMXZ=XZ+SUMXZ 

SUMYX=YX+SUMYX 

SUMYY=YY+SUMYY 

SUMYZ=YZ+SUMYZ 

SUMZX=ZX+SUMZX 

SUMZY=ZY+SUMZY 

c 

100 

SUMZZ=ZZ+SUMZZ 

CONTINUE 

c 
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...  Create  the  CROSSA  matrix 

CR0SSA(1,1)=SUMXX 

CR0SSA(1,2)=SUMXY 

CR0SSA(1,3)=SUMXZ 

CR0SSA(2,1)=SUMYX 

CR0SSA(2,2)=SUMYY 

CR0SSA(2,3)=SUMYZ 

CR0SSA(3,1)=SUMZX 

CR0SSA(3,2)=SUMZY 

CROSSA(3,3)=SUMZZ 

RETURN 

END 


C 
C 

c 
c 

c 
c 
c 
c 
c 
c 
c 
c 
c 


c 
c 
c 


*** 
*** 
*** 
*** 
*** 


■X   *•  K 

#■  *  # 

#  >!  -X 

#  X-  * 

X  *  X 
*** 

#  X-  * 


30 


SUBROUTINE  CONECT  ( OUT, Rl , IND2, K, IND1 , I  A, TESTC, IND2R, DATSET ) 

***  This  subroutine  checks  for  the  connectedness  of  the 
***  input  data  sets.  If  the  problem  is  connected  then  the 
user  is  informed  and  the  array  pairs  are  printed  on  the 
screen;  if  not  connected,  then  the  user  is  prompted  to 
select  one  of  three  options  -  quit,  aad  conecting  data 
sets,  or  run  the  program  using  the  first  connected  set 

that  was  input.  Gygax  -  July  1985 

if********************************************************)'-****** 

...Variable  declarations. 

INTEGERM  R1,K,IND2(2,30),IND1(30,30),I,J,IA(30),FIRST 
INTEGER*4  L I  ST ( 3 0 ) , BEGIN, H ALT, D I  SCON, L , M, 0, TESTC, IND2R(2,3  0) 
INTEGERM  DATSET  (30  )  ,  COUNT,  SAVE  (  2  ,  3  0  )  ,  OUT 

...Initialize  the  values  of  FIRST  and  COUNT: 

FIRST  =  0 
COUNT  =  0 

...Make  vector  IA  =  list  of  all  arrays  (w/o  repeats)  in  I N D 2 
and  get  the  value  for  K  =  #  of  individual  arrays. 

IA(1)  =  IND2(1,1) 
IA(2)  =  IND2(2,1) 
K  =  3 

IF  (Rl  .EQ.  1)  GOTO  60 
DO  50  I  =  1,R1 
DO  40  J  =  1,2 

M  =  K  -  1 

DO  3  0  L  =  1,M 

IF  (IND2(J,I)  .EQ.  IA(D)  GOTO  40 

CONTINUE 
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IA(K) 

K  =  K 

40       CONTINUE 

50       CONTINUE 

60    K  =  K  -  1 


=  IND2(J,I) 
+  1 


C 
C 
C 
C 


70 
80 


131 


140 


141 
150 


151 
160 

170 


WRITE(OUT,*)  »R1   SRI 

WRITE(0UT,*) 'K    »,K 

...  For  each  column  of  IND1  (columns  correspond  to  data  sets)  t\ 
entries  are  all  zero  except  for  the  row  that  corresponds  to 
the  left  array  (=  1)  and  the  right  array  (=  2). 

DO  80  I  =  1,R1 
DO  70  J  =  1,K 

IND1(J,I)  =  0 
IF  (IND2(1,I)  .EQ.  IA(J)) 
IF  (IND2(2,I)  .EQ.  IA(J)) 
CONTINUE 
CONTINUE 


INDKJ,  I) 
IND1(J,I) 


.Check  to  see  if  all  the  arrays  are  connected. 


TESTC  = 
LIST(l) 
DO  131  I 
IF  (I 
IF  (I 
CONTINUE 
BEGIN  = 
HALT  =  1 
IF  (.NOT 
NODE  =  L 
BEGIN  = 
DO  150  I 
IF  (  . 
HA 
LI 
DO 


CO 

CONTINUE 

DC  160  I 

IF  (  . 

HA 

LI 

DO 


CO 

CONTINUE 
GOTO  140 
CONTINUE 


=  -IA(1) 

=  1,R1 
ND2(1,I) 
ND2(2,I) 


.EQ. 
.EQ. 


-LIST(l) ) 
-LIST(l) ) 


IND2(1,I) 
IND2(2, I) 


-IND2(1, I) 
-IND2(2,I) 


.  (BEGIN  .LE.  HALT))  GOTO  170 

IST(BEGIN) 

BEGIN  +  1 

=  1,R1 
NOT. ( (N0DE.EQ.IND2(1, I) ) .AND. (IND2(2, I) .GT.O) ) )  GOTO  150 
LT  =  HALT  +  1 
ST(HALT)  =  -IND2(2,I) 

141  J  =  1,R1 

IF  (IND2(1,J)  .EQ.  -LIST(HALT))  IND2(1,J)  =  -IND2(1,J) 

IF  (IND2(2,J)  .EQ.  -LIST(HALT))  IND2(2,J)  =  -IND2(2,J) 
NTINUE 


=  1,R1 
NOT. ( (NODE.EQ. IND2(2, I) )  .AND.  (IND2(  1,  I)  .GT.O)  ) )  GOTC  160 
LT  =  HALT  +  1 
ST(HALT)  =  -IND2(1,I) 

151  J  =  1,R1 

IF  (IMD2(1,J)  .EQ.  -LIST(HALT))  IND2(1,J) 

IF  (IND2(2,J)  .EQ.  -LIST(HALT))  IND2(2,J) 
NTINUE 


=  -IND2(  1,  J  ) 
=  -IND2(2,J) 


50 


190 


200 


220 

230 
240 


0)  GOTO  190 
0)  GOTO  200 
0)  .AND.  (DISCON 


...Print  out  the  matched  pairs. 
DISCON  =  0 
WRITE(0UT,230) 
DO  200  I  =  1,R1 
IF  (IND2(1,I)  .LT. 
IF  (IND2(1,I)  .EQ. 
IF  ((IND2(1,I)  .GT 
FIRST  =  FIRST  +  1 
DISCON  =  1 
TESTC  =  0 
BEGIN  =  1 
HALT  =  1 

LIST(l)  =  IND2(1,I) 
GOTO  200 

WRITE (OUT, 240)  -IND2(1,I),-IND2(2,I) 
IF  ( ( FIRST. EQ.O) .OR. ( ( FIRST. EQ.l) .AND 
COUNT  =  COUNT  +  1 
IND2R(1, COUNT)  =  -IND2(1,I) 
IND2R(2, COUNT)  =  -IND2(2,I) 
DATSET(COUNT)  =  I 
IF 

-IND2U,  I) 
-IND2(2,I) 
0 


.EQ.  1) )  GOTO  200 


(DISCON. EQ. 1) ) )  THEN 


1)  GOTO  140 


END 
SAVEClf I) 

SAVE(2,I) 
IND2(ltI) 

IND2(2,I)  =  0 

CONTINUE 

IF  (DISCON  .EQ. 

DO  220  I  =  1,R1 

IND2(1,I)  =  SAVEd,  I) 
IND2(2,I)  =  SAVEd, I) 

CONTINUE 

RETURN 

FORMATdX, 'THE  FOLLOWING 

FORMATdX, 1415) 

END 


PAIRS  ARE  CONNECTED  : ' ) 


SUBROUTINE  REDUCE  (CROSSA, MEAN, Rl , K, IND1 , IA, IND2R, DATSET) 

**************************************************  ************ 

*  *  *  This  is  a  specialized  subroutine  that  is  used   when 

option  three  is  involked  as  a  result  of  a  failed  con- 
nectedness test.  The  disconnected  data  sets  must  be 
removed  f ron  the  variables  CROSSA  and  MEAN,  and  ether 
program  supporting  variables  must  be  adjusted. 

Gygax  -  July  1985 
************************************************************** 

...  Variable  declarations. 
INTEGERS  R1,K,  IND1 (30 , 30 ) , IA(30 ) , IND2R( 2,3 0)  ,  I,  J , L, M, DATSET(3  0) 

REAL*8  CROSS  A (30, 3, 3 ), MEAN (3  0, 6) 


*  *  * 
*** 
*** 

*  *  * 
**  * 


*  ** 

*  *  * 

*  *  * 

*  *  * 

*  *  * 


51 


10 
20 


30 


40 
50 
60 


C 

c 
c 
c 
c 
c 


70 
80 


90 


100 
110 
120 


...  Compute  the  new*  reduced  Rl : 
DO  10  I  =  1*30 

IF  (IND2R(1,I)  .EQ.  0)  GOTO  20 
CONTINUE 
Rl  =  I  -  1 

...  Make  a  new*  reduced  vector  IA  =  list  of  all  arrays  in 
IND2R  w/o  repeats.  Also*  compute  a  new  K. 

IA(1)  =  IND2R(1,1) 
IA(2)  =  IND2R(2,1) 
K  =  3 

IF  (Rl  .EQ.  1)  GOTO  60 
DO  50  I  =  1,R1 
DO  40  J  =  1,2 

M  =  K  -  1 

DO  30  L  =  1,M 

IF  (IND2R(J,I)  .EQ.  IA(L))  GOTO  40 

CONTINUE 

IA(K)  =  IND2R(J,I) 

K  =  K  +  1 
CONTINUE 
CONTINUE 
K  =  K  -  1 

...  Remake  the  reduced  matrix  IND1  -  for  each  column  in  IND1 
(corresponding  to  a  data  set)  the  entries  are  zero  except 
for  the  enties  corresponding  to  the  left  array  (=  1)  and 
right  array  (=  2). 

DO  80  I  =  1,R1 
DO  70  J  =  1,K 

IND1(J,I)  =  0 
IF  (IND2R(1,I) 
IF  (IND2R(2,I) 
CONTINUE 
CONTINUE 


EQ.  IA(J)  )  INDKJ,  I)  =  1 
EQ.  IA(J)  )  INDKJ,  I)  =  2 


...  Reduce  the  arrays  CROSSA  and  MEAN  to  account  for  the 
removed  data  sets. 

DO  120  I  =  1,R1 
DO  90  J  =  1,6 

MEAN(I,J)  =  MEAN(DATSET(  I)  , J  ) 
CONTINUE 
DO  110  J  =  1,3 

DO  100  L  =  1,3 

CROSSAd,  J,L)  =  CROSSA(DATSETd)  ,  J,L) 
CONTINUE 
CONTINUE 
CONTINUE 
RETURN 
END 
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SUBROUTINE  POOL ( K, Rl, IND1 , CROSS A, MEAN, NS1 , IND, DEV , XB, R, NS ) 

*************************************************************** 

***   Pools  the  means  and  the  cross  product  deviations        *** 

***   so  that  each  cross  covariance  corresponds  to  a  unique   *** 

***   sensor  array  pair.  *** 

************************************* ************************** 

INTEGER*4  K,R1,IND1(30,30),IND(30,30),R,NS1(30),NS(30),IC,IS 
REAL*8  CR0SSA(30,3,3),  MEAN(30,6),  DEV(30,3,3),  XB(30,6), 
+      TX(6),  DD(3,3),RNS,  DNS1 


...  Outputs  replacement  variables  IND,  DEV,  XB,  R,  and  NS 
for  use  in  MULTAR  and  INTCEP.  That  is: 

IND  replaces  IND1,  DEV  is  the  pooled  version  of  CROSSA, 
XD  is  the  pooled  version  of  MEAN,  R  replaces  Rl, 
and  NS  replaces  NS1. 

...  Identify  data  subsets  in  IND1  which  are  in  the  wrong  orcer 

for  use  in  the  algorithms  and  transpose  thern. 
DO  20  IR=1,R1 
DO  10  1=1, K 

IF  (IND1(I,IR)  .EQ.  0)  THEN 
GOTO  5 
ELSE  IF  (IND1(I,IR)  .EQ.  1)  THEN 

IK=I 
ELSE  IF  (INDKI.IR)  .EQ.  2)  THEN 
JK=I 
END  IF 
5    CONTINUE 
10    CONTINUE 

IF  (IK  .LT.  JK)  GOTO  20 
DO  12  1=1,3 
IP3=I+3 

TX(I)=MEAN(IR, IP3) 
TX(IP3)=MEAN( IR,I) 
DO  12  J=l,3 
12    DD( I, J )  =  CROSSACIR, J, I) 
DO  15  1=1,3 
IP3= 1+3 

MEANdR,  I)=TX(I) 
MEAM( IR, IP3)=TX(IP3) 
DO  15  J=l,3 
15    CROSSACIR, I, J )=DD(  I,  J  ) 
20    CONTINUE 
C      ...  The  transpositions  are  completed. 

C      ...  Locate  the  subsets  to  be  pooled  and  do  the  pooling. 
C  Initialize  the  variables. 

IC  =  1 
C      ...  Zero  the  IND  array. 
DO  25  IT=1,K 
DO  25  IR=1,R1 
IND( IT, IR)=0 
25    CONTINUE 
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c 
c 


c 

c 


:     ...  Start  the  algorithm. 

KM1=K-1 

DO  40  1=1, KM1 

IP1  =  I  +  1 

DO  40  J=IP1,K 

IS  =  0 
;      ...Zero  the  arrays  initially. 

NS(IC)  =  0 

DO  28  IT=1,3 

ITP3=IT+3 

TX(IT)=0.0D+0 

TX(ITP3)=0.0D+0 

DO  28  JT=1,3 
28    DEV(IC, IT,JT)=0.0D+0 

...  Initialization  is  completed  for  this  I ,  J  pair. 

DO  35  IR=1,R1 

IF  dNDld,IR)  .EQ.  0  .OR.  IND1(J,IR)  .EQ.  0)  GOTO  35 

DNS1  =  DBLE(NSKIR)  ) 

...  Set  a  flag  to  show  a  viable  I, J  pair: 

IS=1 

...  Accumulate  weighted  sum  of  means: 

DO  32  IT=1,6 

TX(IT)    =    TX(IT)     +    DNS1    *    MEANdR,IT) 

32  CONTINUE 

I      ...  Accumulate  sums  of  c rossp roducts . 
DO  33  IT=1,3 
DO  33  JT=1,3 
DEVdCt  IT,JT)  =  DEVdC,IT,JT)  +  CR0SSA(  IR,  IT,  J  T) 

33  CONTINUE 

)      ...  Accumulate  sample  sizes: 
NS(  IC)  =  NS(IC)  +  NSKIR) 

35  CONTINUE 

IF  (IS  .EQ.  0)  GOTO  40 
)      ...  Convert  pooled  crossproducts  into  cross  covan'ances. 
RNS  =  DBLE(NSdC)  ) 
DO  36  IT=1,3 
DO  36  JT=1,3 

36  DEV(IC,IT,JT)=DEV(IC,IT,JT)/RNS 

)      ...  Convert  weighted  sums  into  pooled  means: 

DO  38  IT=1,6 
38    XB(ICIT)  =  TX(IT)  /  RNS 

...  Set  the  values  in  the  IND  matrix  and  update  the  counter' 

IND(I,IC)  =  1 

IND( J, IC)  =  2 

IC  =  IC  +  1 
40    CONTINUE 
/      ...  When  finished,  the  counter  needs  correction. 

R=IC  -  1 

RETURN 

END 
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c 

c 
c 
c 
c 
c 
c 
c 
c 
c 


c 
c 


20 


SUBROUTINE  INVIND ( K, R, IND, FF ) 
***************************************************************** 


*** 
*** 
*** 
*** 
*** 
*** 


Computes  the  matrix  FF  from  IND 

FF  is  used  in  MULTAR  and  INTCEP 

IR  indexes  the  columns  of  IND,  i.e.  the  crossover  pairs. 

I  and  J  index  the  sensor  arrays. 

K  is  the  number  of  arrays  in  the  problem 

R  is  the  number  of  (array  pair)  data  sets  in  the  problem. 


INTEGER*4  R,  K 

INTEGER*4  IND(30,30),  FF(30,30) 

...  Initialize  F : 
DO  20  1  =  1, K 
DO  20  J-1,K 

...  Change  the  value  of  FF  to  the  crossover 
those  array  pairs,  I  <  J  ,  that  are  in  the 
FF(I,J)  =  0 


set  index 
problem. 


f  o  r 


KM1  = 
DO  40 
IP1  = 
DO  40 
DO  40 


K  -  1 
1=1, KM1 
I  +  1 
J  =  IP1,K 
IR  =  1,R 

.EQ. 


40 


IF( IND( I, IR) 
CONTINUE 


1  .AND.  IND(J,IR)  .EQ.  2)  FF(I,J)  =  I R 


RETURN 
END 


SUBROUTINE  MULTAR ( EP, FF , DEV , 


K,R,B) 


***************************************************************** 

***  This  is  an  iterative  program  to  compute  the  K  orthonormal  *** 

***  matrices  in  B.  They  must  simultaneously  optimize  the  sum   *** 

***  of  traces  objective  function.  *  *  * 

***************************************************************** 


*  *  K  is  the  number  of  arrays. 

*  *  R  is  the  number  of  crossover  sets. 

*  *  EP  is  the  epsilon  for  stopping  the  iterations. 
**  DEV  is  the  set  of  pooled  crosscovariances. 

**  Calls  BMAX,  MOE  and  MULT3 . 


...  Declarations 

INTEGERM    R,     FF(30,30),     K 

REAL*8       B(30,3,3),     E(30,3,3),     F(30,3,3), 

REAL*8       EP,     REP,     W     ,W0,     BB(3,3),     DD(3,3) 


DEV(30,3,3) 
T(3,3  )  ,     G(30,3,3 ) 
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c 
c 
c 


10 

c 
c 

c 
c 
c 


15 


20 
25 
30 


32 


.  .  Initialize  B  as  K  identity  matrices. 

DO  2  KK=1,K 

DO  2  I  =  1,3 

DO  2  J=l,3 

B(KK,I,J)  =  0.D0 

IF  (I  .EQ.  J)  B(KK,I,J)  =  1.D0 

CONTINUE 

...  Initialize  the  temporary  arrays. 

DO  10  KK=1,K 

DO  10  I  =1,3 

DO  10  J  =  1,3 

F(KK,I,J)=0.D0 

E(KK,I,J)=0.D0 

CONTINUE 

...  Compute  WO,  the  initial  value  of  the  objective  function. 
CALL  MOE(K,FF,DEV,B,W0) 

...  Prepare  for  the  linear  transformation  of  those  members  of  E 

designated  as  LEFT  with  the  cross  covariances. 
DO  30  KK=2,K 
KKM1=KK-1 
DO  30  JI=1,KKM1 
IR=FF(JI,KK) 
IF  (IR  .EQ.  0)  GOTO  25 
DO  15  IB  =  1,3 
DO  15  JB  =  1,3 
BB(IB,  JB)  =  BUI,  IB,JB) 
DD(IB,JB)  =  DEVdR,  IB,JB) 
CONTINUE 

...  Perform  r  a  a  t  r i  x  multiplication  and  accumulate. 

CALL  MULT3(BB,DD,T) 

DO  20  I  =  1,3 

DO  20  J  =  1,3 

E(KK,I,J  )  =  E(KK,  I, J )  +  T( I, J ) 

CONTINUE 

CONTINUE 

...  Prepare  for  the  linear  transformations  cf  those  members  of  i 

designated  as  RIGHT  with  the  cross  covariances. 
KM1  =  K-l 
DO  40  KK=1,KM1 
KKP1  =  KK  +  1 
DO  40  JJ=KKP1,K 
IR=FF(KK, J  J ) 
IF  (IR  .EQ.  0)  GOTO  38 
DO  32  IB  =  1,3 
DO  32  JB  =  1,3 
BB( IB, JB)  =  B( J  J ,  IB, JB) 
DD(  IB, JB)  =  DEV( IR, JB, IB) 
CONTINUE 
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c 
c 


c 
c 


)  ...  Perform  matrix  multiplication  and  accumulate. 

CALL  MULT3(BB,DD,T) 

DO  35  I  =1,3 

DO  35  J  =  1,3 
35      F(KK,I,J)=F(KK,I,J)+T(I,J) 
38      CONTINUE 
40      CONTINUE 

)        ...  Gather  the  LEFT  and  RIGHT  accumulations. 

DO  50  KK=1,K 

DO  50  I  =1,3 

DO  50  J  =  1,3 

G(KK,I, J)=E(KK,I,J)+F(KK,I,J) 
50      CONTINUE 

...  Start  seeking  the  maximum  on  the  objective  surface. 

DO  60  KK=2,K 

DO  55  I  =  1,3 

DO  55  J  =1,3 

BB(I,J)  =  B(KK,I,J) 

DD(  I, J  )  =  G(KK,  I,  J  ) 
55     CONTINUE 

...  Subroutine  call  to  optimize  an  indiviual  matrix  in  B. 

CALL  BMAX(EP,DD,BB) 

...  Insert  updated  values  into  the  B  array. 

DO  58  1=1,3 

DO  58  J=l,3 
58     B(KK, I, J )  =  BB(  I, J ) 
60     CONTINUE 

...  Compute  new  value  of  the  objective  function. 
CALL  MCE(K,FF,DEV,B,W) 

...  Compare  with  previous  value  and  decide  whether  to 

terminate  or  iterate. 
REP  =  EP  *  W0  /  1.D2 
IF  (DABS(W  -  W0)  .GT.  REP)  GOTO  5 


RETURN 
END 


SUBROUTINE  MOE ( K , FF , D E V , B , W ) 

a************************************************************** 

***  Computes  the  measure  of  effectiveness  for  MULTAR:  *  *  * 

***  Output  is  W ,  the  sum  of  traces  of  cross  covariances  *  *  * 

***  modified  on  the  left  by  the  B  matrix  of  the  left  array,  *** 

***  and  on  the  right  by  the  transpose  of  the  B  matrix  of  *** 

***  the  right  array.  *  *  * 


INTEGER*4  K,  FF(30,30) 

REAL*8  BOO, 3, 3),  DEV(30,3,3),  W 
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W=O.DO 

KM1=K-1 

DO  20  IT  =  1,KM1 

ITP1=IT+1 

DO  20  JT  =  ITP1,K 

IR  =  FF(IT,JT) 

IF  (IR  .E'Q.  0)  GOTO  15 

DO  10  1=1,3 

DO  10  M=l,3 

DO  10  J  =  l,3 

W=W+DEV(IR,I,M)*B(JT,J,M)*B(IT,J,I) 
10      CONTINUE 
15      CONTINUE 
20      CONTINUE 


C 


RETURN 
END 


SUBROUTINE  BMAX  (EP,D,B) 
C 

Q  ************************************************************** 

C  ***  Iterative  program  that  climbs  the  surface  D*B-TRANS.  **i 

C  ***  and  stops  when  the  change  is  less  than  EP  tin.es  *** 

C  ***  previous  level  divided  by  10.   Input  D  from  MULTAR  and  *** 

C  ***  output  B  which  is  a  3x3  othonormal  matrix.  *** 

C  ***  Calls  the  subroutine  TWODIM.  *** 

C  ***  --  August  1985.  *** 

C  *****************************************************  ********* 

C 

REAL*8  B(3,3),  D(3,3),  BB(3,3),  C(3,3),  BM(3,3) 

REAL*8  Q,  Q0,  EP,  R,  REP 
C        ...  Initialize  the  value  on  the  surface  Q  for  the  special 
C  case  of  B  =  Identity  matrix: 

Q  =  0.D0 

DO  2  1=1,3 

Q  =  Q  +  D(  I,  I) 
2       CONTINUE 
C 
C        ...  Initialize  B  as  the  identity  matrix: 

DO  5  1=1,3 

DO  5  J=l,3 

B( I, J )  =  0.D0 

IF  (I  .EQ.  J )  B( I, J)  =  1.D0 

5  CONTINUE 

6  CONTINUE 
C 

C        ...  Compute  the  intermediate  values  C  =  D  *    B  -  TRANS  : 
DO  25  I  =  1,3 
DO  25  J  =  1,3 
C( I, J )  =  0.D0 
DO  25  L  =  1,3 

C(I,J)  =  C(I,J)  +  D(I,L)  *  B(J,L) 
25      CONTINUE 
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c 
c 

C        ...  Subroutine  call  to  climb  higher  on  the  surface  of  Q: 

CALL  TWODIM(CBB) 

DO  8  I  =  1,3 

DO  8  J  =  1,3 

BM(I,J)  =  B(I,J) 
8       CONTINUE 
C 
C        ...  Replace  B  with  the  matrix  product.   This  updates  the  B  matrix 

CALL  MULT3(BB,BM,B) 
C 
C        ...  Record  previous  level  on  the  surface: 

QO  =  Q 
C 
C        ...  Compute  new  value  on  the  surface: 

Q  =  0.D0 

DO  10  I  =  1,3 

DO    10    J    =    1,3 
10  Q   =    Q   +    (D(I,J)    *    B(I,J) ) 

C 
C  ...    Prepare    the    stopping    rule: 

R    =    Q   -    QO 

REP  =  EP  *  QO  /  10. 
C        ...  Compares  the  relative  gain  with  EP;  stops  if  too  snail: 

IF  (R.GT.REP)  GOTO  6 


RETURN 
END 


SUBROUTINE  TWODIM  (D,B) 
C 
C        ****************************************************  ******* 

C  ***  FORTRAN  subroutine  to  compute  new  roll,  pitch,  and   *** 

C  ***  yaw  matrices  (B1,B2,B3)  and  combine  into  a  single    *** 

C  ***  orthonormal  transformation.   Receives  D  from  BMAX,   *** 

C  ***  outputs  B,  calls  MULT3 .  *** 

C  ***  --  io  June  1985.  *** 

C  *********************************************************** 

c 

C        ...  Dimension  variables  and  declare  then:  to  be  double  precision 

REAL*8  B(3,3),  D(3,3),  T(3,3),  E(3,3) 

REAL*8  F(3,3),  Bl(3,3),  B2(3,3),  B3(3,3) 

REAL*8  S,  C,  DENOM,  SS 
C 
C        ...  Initialize  the  three  matrices. 

DO  5  1-1,3 

DO  5  J=l,3 

Bl(  I, J )=0.D0 

B2(I,J)=0.D0 

B3(I,J)=0.D0 
5       CONTINUE 
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c 
c 


c 
c 
c 

c 

c 
c 


c 
c 

c 

c 
c 


...  Develop  Bl  (roll  matrix)  from  input  D: 

S  =  D(3,2)  -  D(2,3) 

C  =  D(2,2)  +  D(3,3) 

SS  =  S*S  +  C*C 

DENOM  =  DSQRT(SS) 

S  =  S  /  DENOM 

C  =  C  /  DENOM 

...  Finalize  the  Bl  matrix: 

Bl(2,2)  =  C 

Bl(2,3)  =  S 

Bl(3,2)  =  -S 

Bl(3,3)  =  C 

Bl(l,l)  =  1.D0 

...  Update  the  input  matrix  to  adjust  for  roll: 

. . .  E  =  D*B1 

CALL  MULT3(D,B1,E) 

...  Develop  B2  (pitch  matrix)  from  E: 

S  =  E(3,l)  -  Ed, 3) 

C  =  E(3,3)  +  E(l,l) 

SS  =  S*S  +  C*C 

DENOM  =  DSQRT(SS) 

S  =  S  /  DENOM 

C  =  C  /  DENOM 

...  Finalize  the  B2  matrix: 

B2(l,l)  =  C 

B2(l,3)  =  S 

B2(2,2)  =  1.D0 

B2(3,l)  =  -S 

B2(3,3)  =  C 

...  Additional  update  of  the  input  matrix  to  adjust  for  pitch 

...  F  =  E*B2 

CALL  MULT3(E,B2,F) 

...  Develop  B3  (yaw  matrix)  from  F: 

S  =  F(2,l)  -  F(l,2) 
C  =  F(l,l)  +  F(2,2) 
SS  =  S*S  +  C*C 
DENOM  =  DSQRT(SS) 
S  =  S  /  DENOM 
C  =  C  /  DENOM 

...  Finalize  the  B3  matrix: 

B3(l,l)  =  C 

B3(l,2)  =  S 

B3(2,l)  =  -S 

B3(2,2)  =  C 

B3(3,3)  =  1.D0 
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c 
c 
c 


30 


...  F  i 

w1 
B_ 
DO  30 
DO  30 
B(J,I) 
DO  30 
DO  30 
B(J,I) 
CONTIN 
RETURN 
END 


nal 
th 

t  ra 
1  =  1 
J  =  l 

IK  = 
KJ  = 
=  B( 
UE 


triple  multiplication  of  matrices  coupled 
transposition: 
nspose  =  Bl  *  B2  *  B3 
,3 
,3 

0.D0 
1,3 
1,3 
J#I)+Bl(I#IK)*B2(IKiKJ>*B3(KJ,J) 


SUBROUTINE  MULT3(A,B,C) 


**************************************  **  ******************* 

***   Multiplies  the  3  by  3  matrices  A  and  B  to  get  C     *** 
*********************************************************** 


REAL*8  A(3,3),  B(3,3),  C(3,3) 


10 


DO  10  1=1,3 
DO  10  J-1,3 
C(I,J)=  0.D0 
DO  10  K=l,3 
C(I,J)  =  C(I 
CONTINUE 
RETURN 
END 


J)  +  A(I,K)*B(K,J) 


C 
C 
C 
C 
C 
C 
C 

c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


SUBROUTINE  I NTCEP ( FF , NS , B , XB , K , R, A ) 

**************************************************************** 
*  *  *  Sets  up  and  solves  the  system  of  linear  equations  for  *  *  * 
***  each  component  of  the  set  of  K  intercept  vectors. 
***  Requires  the  output  of  POOL  and  MULTAR. 
***  __  june  1985. 
************************************************************** 


*  *  * 

*  *  * 

*  *  * 


** 
** 
** 
** 
** 
** 
** 
** 
** 
** 
** 
** 


K  is  the  number  of  sensor  arrays  in  the  problem. 
R  is  the  (pooled)  number  of  crossover  data  sets. 
NS  is  the  vector  of  sample  sizes  for  the  crossover-  sets. 
FF  is  the  upper  triangular  K  by  K  matrix  whose  values  are 
either  zero  or  the  index  of  the  data  set  for  the  array  pair- 
identified  by  the  subscripts. 

XB  is  the  R  by  6  matrix  of  means  for  each  of  the  two  arrays 
identified  with  each  of  the  R  data  sets. 

B  is  the  K  by  3  by  3  collection  of  orthonormal  orientation 
correction  matrices  returned  from  MULTAR. 
The  output  A  is  the  K  by  3  matrix  of  intercept  values 
estimated  for  each  of  the  K  arrays  in  the  problem. 
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C       ...  Calls  MMATMUL  which  performs  linear  transformations. 

C       ...  Calls  SYSLIN  which  solves  systems  of  linear  equations. 

C 

C      ...  Decl arat ions. 

C 

INTEGERM    R,    FFO0,30),    NSOO),    K,     I,     IR,     IP1 

REAL*8      BOO, 3, 3),  XB  (30, 6),  MOO, 30), 

1  LHSO0,3),XL(30,3),XLL(30,3),XXL(30,3),XR(30,3),XRR(30,3), 

2  XXR(30,3),    E(30,3),F(30,3),A(30,3),AA(30,31),DNS 
C 

C       ...  Begin  development  of  the  coefficient  matrix  M,  off  diagonal 

C  terms.   Also  prepare  the  inputs  for  the  surnmands  on  the 

C  other  side  of  the  equation.  Affixes  L  and  R  are  used  to 

C  suggest  the  left  and  right  portions  of  the  expressions. 

C 

C       ...  The  values  in  the  coefficient  matrix  are  zero  whenever  the 

C  array  pair  is  not  identified  with  a  crossover  data  set,  i.e 

C  when  FF(i,j)=0  and  i  is  less  than  j.  Otherwise  its  value  is 

C  the  negative  of  the  sample  size  for  the  data  set. 

C 

KM1=K-1 

DO  10  1=1, KM1 

IP1  =  I  +  1 

DO  10  J=IP1,K 

IR=FF(I,J) 

IF  (IP.  .EQ.  0)  THEN 
M( I, J)  =  0.D0 
M(J,I)  =  0.D0 
GOTO  10 

END  IF 

M(I,J)=  -1.D0  *  DBLE(NSdR)  ) 

M  (  J  ,  I )  =  M  (  I ,  J  ) 

DO  5  11=1,3 
C 

C       ...The  weighted  version  requires  that  the  means  be  multiplied 
C  by  NS.  The  first  three  columns  of  XB  are  identified  with  the 

C  left  (L)  array  of  the  pair;  the  last  3  with  the  right  (P.). 

C  This  prepares  inputs  for  surnmands  on  the  other  side  of  the 

C         equation. 
C 

DNS  =  DBLE(NS( IR) ) 

XL( IR, II)=DNS*XB( IR, II) 

IIP3    =3+11 
5  XR( IR, II)=DNS*XB( IR, IIP3 ) 

C 

10  CONTINUE 

C 

C       ...  Finish  development  of  the  V    matrix.  The  diagonal  term  in  a 
C  row  is  the  negative  of  the  total  of  the  other  terms  in  that 

C  row.  The  first  column  of  E  is  used  to  accumulate  terms. 

C 
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DO  15  1=1, K 

M(I,I)  =  O.DO 

EC  1 , 1)  =  O.DO 

DO  12  J  =  1,K 
12     E(I,1)  =  E(I,1)  +  M(ItJ) 

M(I,I)  =  -l.DO  *  E(I,1) 
15     CONTINUE 
C 

C       ...  The  coefficient  matrix  is  complete. 

C  Next  develop  the  sequence  of  partial  sum  arrays  for  use  in 

C  the  LHS  of  the  system  of  equations.  First  initialize. 

C  Then  treat  the  left  array  partial  sums. 

DO  25  KK=2,K 

DO  22  J  =  l,3 

XXL(KK,J)=0.D0 

XLL(KK,J )=0.D0 

22  CONTINUE 
KKM1=KK-1 

DO  25  I=1,KKM1 

IR  =  FF(I,KK) 

IF  (IR  .EQ.  0)  GOTO  25 

CALL  MMATML(K,R,I,IR,IR,B,XL,F) 

DO  23  J  =  l,3 

XXL(KK,J)  =  XXL(KK,J)  +  F(IR,J) 

23  XLL(KK,J)  =  XLL(KK,J)  +  XR(IR,J) 

25  CONTINUE 
C 

C       ...  Next  develop  the  right  array  partial  sums. 
C  Again  the  initial  values  must  be  zero. 

DO  30  KK=1,KM1 

DO  26  1=1,3 

XXR(KK, I)=0.D0 

XRRCKK, I)=0.D0 

26  CONTINUE 
KKP1=KK+1 

DO  30  J=KKP1,K 

IR=FF(KK,J) 

IF  ( IR  .EQ.  0)  GOTO  30 

CALL  MMATML(K,R, J, IR,IR,B,XR,F) 

DO  27  1=1,3 

XXR(KK,I)  =  XXR(KK,I)  +  F(IR,I) 

27  XRR(KK,I)  =  XRR(KK,I)  +  XL(IR,I) 

30  CONTINUE 
C 

C       ...Collect  the  above  quantities  into  the  LHS  of  the  system 

C  of  equations.  Omit  the  end  terms  for  now. 

C 

DO  35  KK=2,KM1 

DO  31  11=1,3 

31  XL(K,II)  =  XLL(KK,II)  +  XRR(KK,II) 
CALL  MNr,ATML(K,R,KK,K,K,B,XL,F) 

DO  33  11=1,3 
33     LHS(KK,II)=XXL(KK,II)+XXR(KK,II)-F(K,II) 
35     CONTINUE 
C 
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c 
c 


c 
c 
c 
c 
c 
c 
c 
c 


c 
c 


...Finalize  with  the  end  correction  terms. 

CALL  MMATML(K,R,1,1,1,B,XRR,F) 

DO  38  I  =  1,3 

LHS(1,I)  =  XXR(1,I)  -  F ( 1 , I ) 
38     CONTINUE 

CALL  MMATML(K,R,K,K,1,B,XLL,F) 

DO  40  1=1,3 
40     LHS(K,I)  =  XXL(K,I)  -  F(1,I) 

...  SOLVE  THE  LINEAR  SYSTEM  MA=L.  This  must  be  done  three  times 
once  for  each  column  of  LHS.  Since  the  matrix  M  has  rank  K- 
and  since  the  solution  is  to  be  made  relative  to  the  first 
array,  we  trim  away  the  first  row  and  first  column  of  M  to 
get  a  non-singular  system.  For  the  same  reason  the  elements 
of  the  first  row  of  A  are  all  set  to  zero. 

DO  50  1=1,3 

...  Prepare  the  input  to  SYSLIN  for  each  column  of  LHS. 

DO  48  KI=2,K 

DO  45  KJ=2,K 

KIM1=KI-1 

KJM1=KJ-1 

AA(KIM1,KJM1)=M(KI,KJ ) 
45     CONTINUE 
48     AA(KIM1,K)=LHS(KI,I) 

...  Solve  the  system. 
CALL  SYSLIN(AA,30,KM1) 
A(1,I)  =  0.D0 

...  Place  the  solution  into  the  output  matrix  A. 
DO  50  KK=2,K 
KKM1=KK-1 

A(KK, I)=AA(KKM1,K) 
50     CONTINUE 


C 
C 


RETURN 
END 


C 
C 
C 

c 
c 
c 
c 

c 
c 


10 


SUBROUTINE  MULMAT( BB , EE , EF ) 

********************************************************** 

***   Subroutine  to  perform  a  linear  transformation  in   *** 

*  *  *   three-space.   Specifically,  EF  is  the  product  of   *** 

***   the  matrix  BB  and  the  vector  EE.  *** 

********************************************************** 

REAL*8  BB(3,3) ,  EF(3)  ,  EE(3) 

...  Initialize  EE  at  zero: 

DO  10  I  =  1,3 
EF(  I)  =  0.D0 
CONTINUE 
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DO  20  I  =  1,3 

X  =  O.DO 
)     .  ...   Accumulate  the  dot  product  of  EE  and  the  I-th  row  of  BB 

DO  30  J  =  1,3 
30     X  =  X  +  BB(I,J)  *  EE(J) 

EF(I)  =  X 
20     CONTINUE 

RETURN 

END 


C 
C 

c 
c 
c 


SUBROUTINE  SYSL IN ( A, IR, IC ) 

*************************************************************** 

***  Finds  the  solution  of  a  system  of  IC  equations  in  IC     *** 
***  unknowns.  *** 

*************************************************************** 


102 


20 


21 


114 
115 


22 


107 


REAL*8   A(l) 

ICP1  =  IC  +  1 

DO  107  K  =  1,IC 

INDEX1  =  (K-1)*IR  +  K 

A(INDEXl)  =  1.D0/ACINDEX1) 

DO  102  J  =  1,ICP1 

IF(J-K)  3,102,3 

INDEX2  =  (J-1)*IR  +  K 

A(INDEX2)  =  A(INDEX2)*A( INDEX1) 

CONTINUE 

DO  115  I  =  1, IC 

IF  (I-K)  20,115,20 

INDEX3  =  (K-1)*IR  +  I 

DO  114  J  =  1,ICP1 

IF(J-K)  21,114,21 


INDEX2  = 
INDEX4  = 


(J-1)*IR  +  I 
(J  -  1)*IR  + 


K 


A(INDEX2)  =  ACINDEX2)  -  A ( I NDEX4 ) *A ( INDEX3 ) 

CONTINUE 

CONTINUE 

DO  107  I  =  1,IC 

IF (  I-K )  22,107,22 

INDEX2  =  (K-1)*IR  + 

A(INDEX2)  =  -1.D0  * 

CONTINUE 

RETURN 

END 


I 

A(  INDEX2) *A( INDEX1) 


65 


SUBROUTINE  MMATML ( K , R, L , M, N , B, E, F ) 


C 
C 
C 
C 
C 
C 
C 
C 
C 
C 
C 
C 
C 
C 
C 
C 

c 
c 
c 


c 

c 
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c 
c 

c 

c 


**** 

*** 

*** 

*** 

*** 

*** 

*** 

*** 

*** 

*** 

*** 

*** 

*** 

*** 

*** 

**** 


***** 

Prep 
Lth 
t  ran 
the 
K  = 
R  = 
L  = 
M  = 
N  = 
B  = 
EE  = 
EF  = 
E  = 
F  = 
***** 


**** 

ares 
face 
sf  or 

new 

numb 

numb 

arra 

i  nde 

i  nde 

Rank 

mul 

p  ro 

i  npu 

outp 
**** 


****** 

for  a 

of  B; 

m  a  t  i  o  n 

vector 

er  of 

er  of 

y  i  n  d  e 

x  of  t 

x  of  t 

3  f am 

t  i  p  1  i  c 

duct  v 

t  arra 

ut  arr 
****** 


************************************ 

linear  transformation  by  a  selected 
calls  MULMAT  to  perform  the 
on  the  Mth  row  of  E  and  positions 
in  the  Nth  row  of  F. 

arrays  in  the  problem 

rows  in  (common  to)  E  and  F 

x;  first  argument  of  B 

he  row  of  E 

he  row  of  F 

ily  of  rotational  matrices 

and  vector 

ector 

y  (of  rank  2 ) ,  row  M  is  used  for  EE 

ay  (of  rank  2  )  ,  E  F  is  placed  in  row  N 

************************************ 


***** 
*** 
*** 
*** 
*** 
**  * 
*** 
*** 
*** 
**  * 
*** 
*** 
*** 
*  -x  * 
*** 

****  * 


20 


INTEGERM  I,  J,  K,  L,  M,  N  ,R 

REAL*8  BBO,3),  BOO, 3, 3),  EEO),  EFO),  EO0,3),  FO0,3) 

...  Initialize  the  temporary  variables: 

DO  10  I  =  1,3 

EE(I)  =  E(M,I) 

DO  10  J  =  1,3 

BB(I,J )  =  B(L,  I,  J  ) 

CONTINUE 

...  Call  routine  to  perform  linear  transformation 

CALL  MULMAT(BB,EE,EF) 

...  Position  the  output  as  prescribed. 

DO  20  I  =  1,3 
F(N,I)  =  EF(I) 
CONTINUE 


RETURN 
END 


C 

C 

c 
c 
c 

c 
c 
c 
c 
c 


SUBROUTINE  DISPLC(K,B,A,IA,DEL,D) 

***************************************************************** 

DEL  is  the  set  of  (output)  displacements  of  the  K  arrays 
estimated  by  the  least  square  method. 
D  is  the  length  of  each  row  of  DEL. 

B  is  the  set  of  K  reorientation  matrices  (output  of  MULTAR) . 
A  is  the  output  of  INTCEP  (i.e.  the  array  intercept  vectors). 
IA  is  the  list  of  sensor  arrays  in  the  problem. 
****** **********x*********** ************************************* 


**  * 

*  ** 
*** 

*  ** 

*  *  * 
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c 
c 


c 
c 
c 


9 
10 


C 

c 
c 
c 
c 
c 
c 


17 


18 


15 


.  . .  Dec! arat i ons . 

REAL*8  BOO, 3, 3),  A(30,3),  AL(30,3),  DEL(30,3), 
REAL*8  ID(3,3),  BB(3,3),  D(30),  ARNUM1,  ARNUM2, 
INTEGERM  IA(30),  K,  DATE 

...  Create  an  identity  matrix  ID: 


AA(3) 
ARNUM3, 


T(3) 


DO  10  1=1,3 
DO  10  J=l,3 
ID(I,J)  =  0 
IF  (  I  .NE. 
ID(I,J)  =  1 
CONTINUE 
CONTINUE 


DO 

J  )  GOTO 

DO 


Read  file  AR1.DAT  in  the  order  specified  by  IA. 

This  is  used  to  create  AL,  the  matrix  of  assumed  locations 

of  those  sensor  arrays  that  appear  in  the  problem. 

The  i dent i cat  ion  vector  IA  produced  in  CONECT  is  needed  to 

extract  the  correct  rows  from  AR1.  The  result  is  A L ( K , 3 ) . 


0PEN( 

J  =  1 

CONTI 

READ( 

GOTO 

CONTI 

WRITE 

WRITE 

STOP 

CONTI 

IF  (I 

A 

A 

A 

J 

R 

ENDIF 

IF(J 


2,FILE='AR1.DAT' , STATU S= ' OLD ' ) 

NUE 

2,*,END=18)   IAR,  DATE,  ARNUM1,  ARNUM2,  ARMUM3 

15 

NUE 

(*,*)  '  Designated  array  number  not  found.  ? 

(*,*)  '  Operation  aborting.  • 

NUE 

AR  .EQ.  IA(J))  THEN 
L(J,1)  =  ARNUM1 
L(J,2)  =  ARNUM2 
L(J,3)  =  ARNUM3 

=  J  +  1 
EWIND  (2) 

.LE.  K)  GOTO  17 


C 

c 


20 


c 
c 
c 


22 


CLOSE  (UNIT=2) 

...  Compute  the  displacements. 

...  First  reduce  by  one  the  diagonal  elements  of  each  fc.ce 

DO  30  KK=1,K 

DO  20  1=1,3 

DO  20  J=l,3 

BB(  I,  J)  =  B(KK,  I,  J  )  -  IDC  I,  J  ) 

CONTINUE 


...  Complete  the  displacement  computation: 

DO  22  1=1,3 

AA( I)  =  AL(KK,  I) 

CALL  MULMATCBB, AA,T) 


of  E 
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DO  25  1=1,3 

DEL(KK,I)=  A(KK,I)+T(I) 
25    CONTINUE 
30    CONTINUE 
C 

C       ...  Compute  the  length  of  each  row  of  DEL. 
C 

DO  40  KK=1,K 

D(KK)  =  0.D0 

DO  35  1=1,3 

D(KK)  =  D(KK)  +  (DEL(KK,I)  *  DEL(KK,I)) 
35    CONTINUE 
40    D(KK)  =  DSQRT(D(KK)) 

RETURN 

END 

SUBROUTINE  MAXANG(BB,P) 

REAL*8  B(3,3),  BB(3,3),  X(3),  Y(3),  D,  P 
C 

Q  ************************************************************** 

C       *  *  *  Takes  the  orthonormal  matrix  BB  as  input  and  constructs  *** 

C       ***  the  angle  of  maximal  rotation  that  any  vector  can        *** 

C        ***  experience  under  the  transformation  BB.  *** 

C        ***  Calls  MULT3.  *** 

C        *************************************************************** 

C 

C        ...  Begin  with  the  adjustment  of  the  diagonal  elements  of  E 

C  in  order  to  prepare  the  eigenvector  problem. 

C 

DO  5  1=1,3 

DO  5  J=l,3 

B(I,J)  =  BB(I,J ) 

IF  (I  .EQ.  J )  B(  I, I)  =  B(I, I)  -  1. 
5       CONTINUE 
C        ...  Use  the  first  two  equations  of  the  eigenvector  system 
C  (with  X3  =  1)  to  solve  for  the  eigenvector. 

C 
C        ...  Compute  the  determinant  of  the  coefficient  matrix; 

D  =  B(l,l)*6(2,2)  -  B(1,2)*B(2,1) 
C        ...  Check  for  zero  rotation  and  transfer  to  the  end 
C  if  it  occurs. 

IF  (D  .EQ.  0.0)  THEN 
D  =  1 .DO 
GOTO  30 

END  IF 
C 

C        ...  Set  third  component  equal  to  one  and  solve  the  linear- 
C  system  for  the  other  two. 

X(3)  =  1.D0 

X(l)  =  (B(1,2)*B(2,3)  -  B(1,3)*B(2,2) )/D 

X(2)  =  (B(1,3)*B(2,1)  -  B(1,1)*B(2,3) )/D 
C 

C        ...  Normalize  the  eigenvectors. 
C 
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c 
c 
c 
c 


c 
c 
c 


D  =  DSQRTd.DO  +  X(1)*X(1)  +  X(2)*X(2)) 
DO  10  1=1,3 
10      X(I)  =  X(I)/D 

...  Construct  a  vector  orthogonal  to  the  eigenvector. 
First  choose  the  subscript  of  the  smallest  X. 

J  =  l 

IF  (DABS(Xd))  .GT.  DABS(X(2)))  J  =  2 

IF  (DABS(X(J))  .GT.  DABS(X(3)))  J=3 

...  Use  the  Gram-Schmidt  technique  to  convert  the  direction 

of  X(J)  to  a  direction  orthogonal  to  X(l),  X(2),  X(3). 
DO  15  1=1,3 
IF  (I  .EQ.  J)  THEN 

Y(J )  =   X(J)*d.-  X(J)*X(J ) ) 
ELSE 

Yd)  =  (-1.  )*Xd)*X(J)*X(J) 
END  IF 
15      CONTINUE 
!        ...  Normalize  the  new  vector. 

D  =  DSQRT(  Yd)*Yd)  +  Y(2)*Y(2)  +  Y(3)*Y(3)  ) 
DO  20  1=1,3 
20      Yd)  =  Y(I)/D 

...  Transform  Y  by  BB  and  compute  the  angle  between  Y  and  B B * Y 

CALL  MULMAT(BB,Y,X) 

D  =  0.D0 

DO  25  1=1,3 

D  =  D  +  X( I ) *Y(  I ) 

25      CONTINUE 

30      P  =  DACOS(D) 

RETURN 

END 


SUBROUTINE  ANGLE S ( K , B , P ) 

****************************************************  ***** 

***  Computes  the  three  Euler  angles  for  eech  of  the    *  *  * 
***  K  matrices  in  B.  *** 

********************************************************* 

REAL*8  B(30,3,3 ) ,P(30,3 ) 


20 


DO  20  KK=1,K 

P(KK,2)=DASIN  (B(KK,3,D) 

P(KK,1)=DASIN  (B(KK,3,2)/DCCS  (P(KK,2))) 

P(KK,3)=DASIN  (B(KK,2,1)/DC0S  (P(KK,2))) 

CONTINUE 

RETURN 

END 
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